From ad385498f5c4adbf157f7e4f38c4ccab6fe89bd2 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Mon, 12 Sep 2016 17:42:22 -0700 Subject: [PATCH 001/183] Added GPS to state estimation. Adapted state estimation (individual measurement noise for all measurements) and added launch files for estimation. --- .../barc/launch/estimation_KinBkMdl.launch | 5 +- .../launch/estimation_KinBkMdl_gps.launch | 42 +++++++++ .../src/barc/src/state_estimation_KinBkMdl.py | 85 +++++++++++++++---- workspace/src/barc/src/system_models.py | 9 +- 4 files changed, 122 insertions(+), 19 deletions(-) mode change 100644 => 100755 workspace/src/barc/launch/estimation_KinBkMdl.launch create mode 100755 workspace/src/barc/launch/estimation_KinBkMdl_gps.launch mode change 100644 => 100755 workspace/src/barc/src/system_models.py diff --git a/workspace/src/barc/launch/estimation_KinBkMdl.launch b/workspace/src/barc/launch/estimation_KinBkMdl.launch old mode 100644 new mode 100755 index c9cfb374..7640020d --- a/workspace/src/barc/launch/estimation_KinBkMdl.launch +++ b/workspace/src/barc/launch/estimation_KinBkMdl.launch @@ -2,6 +2,7 @@ + @@ -24,7 +25,9 @@ - + + + diff --git a/workspace/src/barc/launch/estimation_KinBkMdl_gps.launch b/workspace/src/barc/launch/estimation_KinBkMdl_gps.launch new file mode 100755 index 00000000..d4b92421 --- /dev/null +++ b/workspace/src/barc/launch/estimation_KinBkMdl_gps.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py index 46aba05f..0f076df9 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl.py @@ -17,25 +17,28 @@ import time import os from sensor_msgs.msg import Imu +from geometry_msgs.msg import Vector3 from barc.msg import ECU, Encoder, Z_KinBkMdl from numpy import pi, cos, sin, eye, array, zeros, unwrap -from ekf import ekf +from observers import kinematicLuembergerObserver, ekf from system_models import f_KinBkMdl, h_KinBkMdl from tf import transformations -from numpy import unwrap +from numpy import unwrap, diag # input variables [default values] d_f = 0 # steering angle [deg] acc = 0 # acceleration [m/s] # raw measurement variables -yaw_prev = 0 +yaw_prev = 0 (roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) yaw_prev = 0 yaw_local = 0 read_yaw0 = False psi = 0 psi_meas = 0 +x_meas = 0 +y_meas = 0 # from encoder v = 0 @@ -49,9 +52,10 @@ n_FR_prev = 0 n_BL_prev = 0 n_BR_prev = 0 -r_tire = 0.036 # radius from tire center to perimeter along magnets [m] +r_tire = 0.036 # radius from tire center to perimeter along magnets [m] dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge [m] + # ecu command update def ecu_callback(data): global acc, d_f @@ -69,6 +73,7 @@ def imu_callback(data): ori = data.orientation quaternion = (ori.x, ori.y, ori.z, ori.w) (roll, pitch, yaw) = transformations.euler_from_quaternion(quaternion) + # yaw = -yaw # added for wrong coordinate frame # save initial measurements if not read_yaw0: @@ -90,6 +95,13 @@ def imu_callback(data): a_y = data.linear_acceleration.y a_z = data.linear_acceleration.z +# ultrasound gps data +def gps_callback(data): + # units: [rad] and [rad/s] + global x_meas, y_meas + x_meas = data.x/100 # data is given in cm + y_meas = data.y/100 + # encoder measurement update def enc_callback(data): global v, t0, dt_v_enc, v_meas @@ -116,7 +128,7 @@ def enc_callback(data): # Uncomment/modify according to your encoder setup # v_meas = (v_FL + v_FR)/2.0 # Modification for 3 working encoders - v_meas = (v_FL + v_BL + v_BR)/3.0 + v_meas = (v_FL + v_BL + v_BR)/2.0 # Modification for bench testing (driven wheels only) # v = (v_BL + v_BR)/2.0 @@ -125,13 +137,13 @@ def enc_callback(data): n_FR_prev = n_FR n_BL_prev = n_BL n_BR_prev = n_BR - t0 = time.time() - + t0 = tf # state estimation node def state_estimation(): global dt_v_enc global v_meas, psi_meas + global est_mode # initialize node rospy.init_node('state_estimation', anonymous=True) @@ -139,11 +151,16 @@ def state_estimation(): rospy.Subscriber('imu/data', Imu, imu_callback) rospy.Subscriber('encoder', Encoder, enc_callback) rospy.Subscriber('ecu', ECU, ecu_callback) + rospy.Subscriber('indoor_gps', Vector3, gps_callback) state_pub = rospy.Publisher('state_estimate', Z_KinBkMdl, queue_size = 10) # get vehicle dimension parameters L_a = rospy.get_param("L_a") # distance from CoG to front axel L_b = rospy.get_param("L_b") # distance from CoG to rear axel + est_mode = rospy.get_param("est_mode") # estimation mode + # mode 1: gps, IMU for orientation and encoders for v + # mode 2: no gps, IMU for orientation and encoders for v, x, y + # mode 3: only pgps, no IMU, no encoders (experimental) vhMdl = (L_a, L_b) # get encoder parameters @@ -151,8 +168,15 @@ def state_estimation(): # get EKF observer properties q_std = rospy.get_param("state_estimation/q_std") # std of process noise - r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise - + psi_std = rospy.get_param("state_estimation/psi_std") # std of measurementnoise + v_std = rospy.get_param("state_estimation/v_std") + gps_std = rospy.get_param("state_estimation/gps_std") # std of gps measurements + + t = time.localtime(time.time()) + file = open("/home/odroid/rosbag/info_%i_%02i_%02i_%02i_%02i_%02i.txt"%(t.tm_year,t.tm_mon,t.tm_mday,t.tm_hour,t.tm_min,t.tm_sec),'w') + file.write("EKF parameters\n===============\n") + file.write("q = %f\ngps_std = %f\npsi_std = %f\nv_std = %f"%(q_std,gps_std,psi_std,v_std)) + file.close() # set node rate loop_rate = 50 dt = 1.0 / loop_rate @@ -165,25 +189,56 @@ def state_estimation(): # estimation variables for EKF P = eye(4) # initial dynamics coveriance matrix Q = (q_std**2)*eye(4) # process noise coveriance matrix - R = (r_std**2)*eye(2) # measurement noise coveriance matrix - + # R = (r_std**2)*eye(4) # measurement noise coveriance matrix + if est_mode==1: + R = diag([gps_std,gps_std,psi_std,v_std])**2 + elif est_mode==2: + R = diag([psi_std,v_std])**2 + elif est_mode==3: + R = (gps_std**2)*eye(2) + else: + rospy.logerr("No estimation mode selected.") + + count = 0 + x_prev = 0 + y_prev = 0 + + # start loop while not rospy.is_shutdown(): # publish state estimate - (x, y, psi, v) = z_EKF + (x_e, y_e, psi_e, v_e) = z_EKF # publish information - state_pub.publish( Z_KinBkMdl(x, y, psi, v) ) + state_pub.publish( Z_KinBkMdl(x_e, y_e, psi_e, v_e) ) # collect measurements, inputs, system properties + + # check gps values for *single* outlier + if count > 100: # only if estimation is already about right + if est_mode == 1 or est_mode == 3: # if gps is included in the measurements + gps_diff = array([x_meas-x_e,y_meas-y_e]) # vector difference between estimate and gps position + if (gps_diff[0]**2+gps_diff[1]**2)**0.5 > 0.8: # outlier distance: 0.8m + x_meas = x_prev + y_meas = y_prev + + if est_mode==1: + y = array([x_meas, y_meas, psi_meas, v_meas]) + elif est_mode==2: + y = array([psi_meas, v_meas]) + elif est_mode==3: + y = array([x_meas, y_meas]) + # collect inputs - y = array([psi_meas, v_meas]) u = array([ d_f, acc ]) args = (u,vhMdl,dt) - # apply EKF and get each state estimate (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) + x_prev = x_meas + y_prev = y_meas + count = count + 1 + # wait rate.sleep() diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py old mode 100644 new mode 100755 index 1893eb55..e3e05d12 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -197,7 +197,8 @@ def f_KinBkMdl(z,u,vhMdl, dt): input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] output: state at next time step z[k+1] """ - + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) + # get states / inputs x = z[0] y = z[1] @@ -217,7 +218,7 @@ def f_KinBkMdl(z,u,vhMdl, dt): x_next = x + dt*( v*cos(psi + bta) ) y_next = y + dt*( v*sin(psi + bta) ) psi_next = psi + dt*v/L_b*sin(bta) - v_next = v + dt*a + v_next = v + dt*(a - 0.63*sign(v)*v**2) return array([x_next, y_next, psi_next, v_next]) @@ -225,7 +226,9 @@ def h_KinBkMdl(x): """ measurement model """ - C = array([[0, 0, 1, 0], + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], [0, 0, 0, 1]]) return dot(C, x) From 1267bef105784f7fb56f9a7a275f1687dc2059f3 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Mon, 12 Sep 2016 20:42:48 -0700 Subject: [PATCH 002/183] Added DS_Store to gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index d4fabeb1..b02f9446 100644 --- a/.gitignore +++ b/.gitignore @@ -65,3 +65,4 @@ controller.py Dator/db.sqlite3 workspace/src/barc/rosbag/ *.ropeproject/ +.DS_Store From c74749269c239ea80b552d879f2e25addc25aaa3 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Mon, 12 Sep 2016 20:48:22 -0700 Subject: [PATCH 003/183] Added Localization node --- .../src/barc/src/Localization_helpers.py | 222 ++++++++++++++++++ workspace/src/barc/src/Localization_node.py | 67 ++++++ 2 files changed, 289 insertions(+) create mode 100755 workspace/src/barc/src/Localization_helpers.py create mode 100755 workspace/src/barc/src/Localization_node.py diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py new file mode 100755 index 00000000..b22cb43b --- /dev/null +++ b/workspace/src/barc/src/Localization_helpers.py @@ -0,0 +1,222 @@ +from numpy import *#sin, cos, tan, arctan, array, dot, linspace, pi, ones, zeros, sum, amin, argmin +import scipy.optimize + +def create_circle(rad,n,c): + ang = linspace(0,2*pi,n) + x = rad*cos(ang)+c[0] + y = rad*sin(ang)+c[1] + return array([x,y]) + +class Localization: + n = 0 # number of nodes + c = 0 # center of circle (in case of circular trajectory) + pos = 0 # current position + psi = 0 # current orientation + nodes = 0 # all nodes are saved in a matrix + N_nodes_poly_back = 10 # number of nodes behind current position + N_nodes_poly_front = 30 # number of nodes in front + ds = 0 # distance between nodes + nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total + OrderXY = 6 # order of x-y-polynomial interpolation + OrderThetaCurv = 3 # order of theta interpolation + closed = True # open or closed trajectory? + + coeffX = 0 + coeffY = 0 + coeffTheta = 0 + coeffCurvature = 0 + + s = 0 # distance from s_start to current closest node (idx_min) + s_start = 0 # distance along path from first node to start node (which is N_nodes_poly_back behind current closest node) + ey = 0 # lateral distance to path + epsi = 0 # error in psi (between current psi and trajectory tangent) + v = 0 # current velocity (not necessary for these calculations but for MPC) + + def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to make sure that all points are equidistant! + ang = linspace(0,2*pi-2*pi/n,n) + #print(ang) + x = rad*cos(ang)+c[0] + y = rad*sin(ang)+c[1] + self.nodes = array([x,y]) + self.n = n + self.c = c + self.rad = rad + #self.ds = rad*2*pi/n + self.ds = 2*rad*tan(2*pi/n/2) + def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points + x = arange(-L/2.0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines + s = size(x) + da = 2*pi/360*5 + x = hstack((x,L/2.0+b/2*cos(arange(pi/2,-pi/2,-da)))) + x = hstack((x,arange(L/2.0,-L/2.0,-ds))) + x = hstack((x,-L/2.0+b/2*cos(arange(pi/2,1.5*pi+da,da)))) + + y = ones(s)*b/2.0 + y = hstack((y,b/2*sin(arange(pi/2,-pi/2,-da)))) + y = hstack((y,-ones(s)*b/2)) + y = hstack((y,-b/2*sin(arange(pi/2,1.5*pi+da,da)))) + + s_tot = size(x) + + R = array([[cos(ang),-sin(ang)],[sin(ang),cos(ang)]]) # Rotation Matrix + + nodes = dot(R,array([x,y]))+(c*ones([2,s_tot]).T).T + + self.nodes = nodes + self.n = size(x) + self.ds = ds + + def create_ellipse(self,L_a,L_b,n,c=array([0,0])): + ang = linspace(0,2*pi,n) + x = L_a*cos(ang)+c[0] + y = L_b*sin(ang)+c[1] + self.nodes = array([x,y]) + self.n = n + self.c = c + + def prepare_trajectory(self,ds_in): # brings trajectory to correct format (equidistant nodes with distances around ds) + x = self.nodes[0] + y = self.nodes[1] + n = size(x) # number of nodes + ds = (diff(x)**2+diff(y)**2)**0.5 # distances between nodes + #if self.closed: + # ds = hstack((ds,((x[n-1]-x[0])**2+(y[n-1]-y[0])**2)**0.5)) + s = hstack((0,cumsum(ds))) + length = sum(ds) + dsn = ds_in # optimal new step size (might be calculated externally for now) + rem = lambda x:mod(length,x) # function to calculate optimal step size (for evenly distributed steps) + sol = scipy.optimize.fmin(func=rem,x0=dsn) + dsn = sol[0] + sn = arange(0,length,dsn) # new steps + xn = interp(sn,s,x) + yn = interp(sn,s,y) + + self.nodes = array([xn,yn]) + self.ds = dsn + self.n = size(xn) + + + def set_pos(self,x,y,psi,v): + self.pos = array([x,y]) + self.psi = psi + self.v = v + + def find_s(self): + dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of pos to all nodes + #mindist = amin(dist) # minimum distance + idx_min = argmin(dist) # index of minimum distance + n = self.n + nPoints = self.nPoints + # Use closest node to determine start and end of polynomial approximation + idx_start = idx_min - self.N_nodes_poly_back + idx_end = idx_min + self.N_nodes_poly_front + if self.closed == True: + if idx_start<0: + nodes_X = hstack((self.nodes[0,n+idx_start:n],self.nodes[0,0:idx_end+1])) + nodes_Y = hstack((self.nodes[1,n+idx_start:n],self.nodes[1,0:idx_end+1])) + idx_start = n+idx_start + elif idx_end>n-1: + nodes_X = hstack((self.nodes[0,idx_start:n],self.nodes[0,0:idx_end+1-n])) + nodes_Y = hstack((self.nodes[1,idx_start:n],self.nodes[1,0:idx_end+1-n])) + else: + nodes_X = self.nodes[0,idx_start:idx_end+1] + nodes_Y = self.nodes[1,idx_start:idx_end+1] + else: + if idx_start<0: + nodes_X = self.nodes[0,0:nPoints] + nodes_Y = self.nodes[1,0:nPoints] + idx_start = 0 + elif idx_end>n-1: + nodes_X = self.nodes[0,n-nPoints:n] + nodes_Y = self.nodes[1,n-nPoints:n] + idx_start = n-nPoints + else: + nodes_X = self.nodes[0,idx_start:idx_end+1] + nodes_Y = self.nodes[1,idx_start:idx_end+1] + + # Create Matrix for interpolation + # x-y-Matrix + Matrix = zeros([self.nPoints,self.OrderXY+1]) + for i in range(0,self.nPoints): + for k in range(0,self.OrderXY+1): + Matrix[i,self.OrderXY-k] = (i*self.ds)**k + + # curvature matrix + Matrix3rd = zeros([self.nPoints,self.OrderThetaCurv+1]) + for i in range(0,self.nPoints): + for k in range(0,self.OrderThetaCurv+1): + Matrix3rd[i,self.OrderThetaCurv-k] = (i*self.ds)**k + + # Solve system of equations to find polynomial coefficients (for x-y) + self.coeffX = linalg.lstsq(Matrix,nodes_X)[0] + self.coeffY = linalg.lstsq(Matrix,nodes_Y)[0] + + # find angles and curvature along interpolation polynomial + b_theta_vec = zeros(self.nPoints) + b_curvature_vector = zeros(self.nPoints) + + for j in range(0,self.nPoints): + s = j*self.ds + dX = polyval(polyder(self.coeffX,1),s) + dY = polyval(polyder(self.coeffY,1),s) + ddX = polyval(polyder(self.coeffX,2),s) + ddY = polyval(polyder(self.coeffY,2),s) + angle = arctan2(dY,dX) + if j>1: + if angle - b_theta_vec[j-1] > pi: + angle = angle - 2*pi + elif angle - b_theta_vec[j-1] < -pi: + angle = angle + 2*pi + + b_theta_vec[j] = angle + b_curvature_vector[j] = (dX*ddY-dY*ddX)/(dX**2+dY**2)**1.5 + + # calculate coefficients for Theta and curvature + coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] + coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] + + # Calculate s + discretization = 0.01 + s_idx_start = max(0,idx_min-1) + if self.closed and s_idx_start-idx_start<0: + s_idx_start = s_idx_start + n + j = arange((s_idx_start-idx_start)*self.ds,(s_idx_start-idx_start+2)*self.ds,discretization) + x_eval = polyval(self.coeffX,j) + y_eval = polyval(self.coeffY,j) + dist_eval = sum((self.pos*ones([size(j),2])-vstack((x_eval,y_eval)).transpose())**2,1)**0.5 + idx_s_min = argmin(dist_eval) + s = j[idx_s_min] # s = minimum distance to points between idx_min-1 and idx_min+1 + eyabs = amin(dist_eval) # absolute distance to curve + + # Calculate sign of y + s0 = s - discretization + XCurve0 = polyval(self.coeffX,s0) + YCurve0 = polyval(self.coeffY,s0) + XCurve = polyval(self.coeffX,s) + YCurve = polyval(self.coeffY,s) + dX = polyval(polyder(self.coeffX,1),s) + dY = polyval(polyder(self.coeffY,1),s) + + xyVectorAngle = arctan2(self.pos[1]-YCurve0, self.pos[0]-XCurve0) + xyPathAngle = arctan2(dY,dX) + ey = eyabs*sign(sin(xyVectorAngle-xyPathAngle)) + + # Calculate epsi + epsi = self.psi-xyPathAngle + if abs(epsi) > pi/2: + if epsi < pi/2: + epsi = epsi + 2*pi + else: + epsi = epsi - 2*pi + + self.epsi = epsi + self.ey = ey + self.s = s + self.coeffTheta = coeffTheta + self.coeffCurvature = coeffCurvature + self.s_start = idx_start*self.ds + + + def __init__(self): + self.x = 0 + self.y = 0 diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py new file mode 100755 index 00000000..2ed4c475 --- /dev/null +++ b/workspace/src/barc/src/Localization_node.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# +# --------------------------------------------------------------------------- + + +import rospy +import time +import os +from numpy import * +from Localization_helpers import Localization +from barc.msg import Z_KinBkMdl, pos_info + +l = 0 +epsi_prev = 0 + +# State estimate callback function +def state_estimate_callback(data): + global l, epsi_prev + # Set current position and orientation + l.set_pos(data.x,data.y,data.psi,data.v) + # Update position and trajectory information + l.find_s() + + # unwrap epsi + l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] + epsi_prev = l.epsi; + +# localization node +def localization_node(): + global l # localization class + + # initialize node + rospy.init_node('localization', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback) + state_pub = rospy.Publisher('pos_info', pos_info, queue_size = 10) + + # create localization class and trajectory + l = Localization() + # l.create_circle(1,100,array([3.2,0.5])) + l.create_racetrack(2.5,1.5,0.2,array([3.5,1.0]),0) + # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) + l.prepare_trajectory(0.063) + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + t0 = time.time() + + while not rospy.is_shutdown(): + + # publish information + state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.coeffX,l.coeffY,l.coeffTheta,l.coeffCurvature) ) + + # wait + rate.sleep() + + +if __name__ == '__main__': + try: + localization_node() + except rospy.ROSInterruptException: + pass From 7cbb91f87dc2d018d82dcf1f6a40605ffee6efc6 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Mon, 12 Sep 2016 21:04:49 -0700 Subject: [PATCH 004/183] Added first draft of LMPC node. Not tested yet. --- workspace/src/barc/launch/lmpc.launch | 38 ++++ workspace/src/barc/src/LMPC_node.jl | 122 +++++++++++ workspace/src/barc/src/helper/classes.jl | 77 +++++++ .../barc/src/helper/coeffConstraintCost.jl | 192 ++++++++++++++++++ .../src/barc/src/helper/computeCostLap.jl | 9 + workspace/src/barc/src/helper/createModel.jl | 99 +++++++++ .../src/barc/src/helper/solveMpcProblem.jl | 133 ++++++++++++ 7 files changed, 670 insertions(+) create mode 100644 workspace/src/barc/launch/lmpc.launch create mode 100644 workspace/src/barc/src/LMPC_node.jl create mode 100644 workspace/src/barc/src/helper/classes.jl create mode 100644 workspace/src/barc/src/helper/coeffConstraintCost.jl create mode 100644 workspace/src/barc/src/helper/computeCostLap.jl create mode 100644 workspace/src/barc/src/helper/createModel.jl create mode 100644 workspace/src/barc/src/helper/solveMpcProblem.jl diff --git a/workspace/src/barc/launch/lmpc.launch b/workspace/src/barc/launch/lmpc.launch new file mode 100644 index 00000000..f61474ef --- /dev/null +++ b/workspace/src/barc/launch/lmpc.launch @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl new file mode 100644 index 00000000..f41707b1 --- /dev/null +++ b/workspace/src/barc/src/LMPC_node.jl @@ -0,0 +1,122 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using JuMP +using Ipopt + +include("helper/classes.jl") +include("helper/coeffConstraintCost.jl") +include("helper/solveMpcProblem.jl") +include("helper/computeCostLap.jl") +include("simModel.jl") + +# Load Variables and create Model: +println("Loading and defining variables...") +include("createModel.jl") + +# Initialize model by solving it once +println("Initial solve...") +solve(mdl) +println("Finished initial solve.") + +QderivZ = 0.0*[1 1 1 1] # cost matrix for derivative cost of states +QderivU = 0.1*[1 1] # cost matrix for derivative cost of inputs +mpcParams.R = 0.0*[1 1] # cost matrix for control inputs +mpcParams.Q = [0.0 10.0 1.0 1.0] # put weights on ey, epsi and v + +cost = zeros(length(t),6) +posInfo.s_target = 6 + +trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) +z_final = zeros(1,4) +u_final = zeros(1,2) + +z_est = zeros(1,4) + +function SE_callback(msg::pos_info) # update current position and track data + # update mpc initial condition + z_est = [msg.s msg.ey msg.epsi msg.v] + posInfo.s_start = msg.s_start + trackCoeff.coeffCurvature = msg.coeffCurvature +end + +function main() + # initiate node, set up publisher / subscriber topics + init_node("mpc_traj") + pub = Publisher("ecu", ECU, queue_size=10) + pub2 = Publisher("logging", Logging, queue_size=10) + s1 = Subscriber("pos_info", pos_info, SE_callback, queue_size=10) + loop_rate = Rate(10) + + lapStatus.currentLap = 1 # initialize lap number + lapStatus.currentIt = 1 # current iteration in lap + switchLap = false # initialize lap lap trigger + s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] + + zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) + uCurr = zeros(10000,2) # contains input information + + while ! is_shutdown() + lapStatus.currentIt += 1 # count iteration + i = lapStatus.currentIt # just to make notation shorter + zCurr[i,:] = z_est # update state information + + # Find coefficients for cost and constraints + mpcCoeff = coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + # Solve the MPC problem + mpcSol = solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i-1,:]',uCurr[i-1,:]') + # Write in current input information + uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] + # ... and publish data + cmd = ECU(mpcSol.a_x, mpcSol.d_f) + publish(pub, cmd) + + # ================================== + # Lap trigger ====================== + # ================================== + if posInfo.s_start < s_lapTrigger && switchLap # if we are switching to the next lap... + # ... then select and save data + zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) + uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) + costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line + # Save all data in oldTrajectory: + if lapStatus.currentLap == 1 # if it's the first lap + oldTraj.oldTraj[:,:,1] = zCurr_export' # ... just save everything + oldTraj.oldInput[:,:,1] = uCurr_export' + oldTraj.oldTraj[:,:,2] = zCurr_export' + oldTraj.oldInput[:,:,2] = uCurr_export' + oldTraj.oldCost = [costLap,costLap] + else # idea: always copy the new trajectory in the first array! + if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second + oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second + oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input + oldTraj.oldCost[2] = oldTraj.oldCost[1] + end + oldTraj.oldTraj[:,:,1] = zCurr_export' # ... and write the new traj in the first + oldTraj.oldInput[:,:,1] = uCurr_export' + oldTraj.oldCost[1] = costLap + end + + lapStatus.currentLap += 1 # start next lap + lapStatus.currentIt = 1 # reset current iteration + switchLap = false + zCurr[1,:] = zCurr[i,:] + uCurr[1,:] = uCurr[i,:] + else + switchLap = true + end + + rossleep(loop_rate) + end +end + +if ! isinteractive() + main() +end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl new file mode 100644 index 00000000..ceb1e542 --- /dev/null +++ b/workspace/src/barc/src/helper/classes.jl @@ -0,0 +1,77 @@ +# VARIOUS TYPES FOR CALCULATIONS + +type LapStatus + currentLap::Int64 # current lap number + currentIt::Int64 # current iteration in current lap +end + +# Structure of coeffConst: +# 1st dimension specifies the state (1 = eY, 2 = ePsi, 3 = v) +# 2nd dimension is the polynomial coefficient +# 3rd dimension is not used +# 4th dimension specifies one of the two lap numbers between which are iterated + +type MpcCoeff # coefficients for trajectory approximation + coeffCost + coeffConst + order + pLength # small values here may lead to numerical problems since the functions are only approximated in a short horizon + # "small" values are about 2*N, good values about 4*N + # numerical problems occur at the edges (s=0, when v is almost 0 and s does not change fast and at s=s_target) + MpcCoeff(coeffCost=0, coeffConst=0, order=4, pLength=0) = new(coeffCost, coeffConst, order, pLength) +end + +type OldTrajectory # information about previous trajectories + oldTraj + oldInput + oldCost + OldTrajectory(oldTraj=0,oldInput=0,oldCost=0) = new(oldTraj,oldInput,oldCost) +end + +type MpcParams # parameters for MPC solver + N::Int64 + nz::Int64 + OrderCostCons::Int64 + Q + R + vPathFollowing::Float64 + MpcParams(N=0,nz=0,OrderCostCons=0,Q=0,R=0,vPathFollowing=1.0) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) +end + +type PosInfo # current position information + s_start::Float64 + s::Float64 + s_switch::Float64 + s_target::Float64 + PosInfo(s_start=0,s=0,s_switch=0,s_target=0) = new(s_start,s,s_switch,s_target) +end + +type MpcSol # MPC solution output + a_x::Float64 + d_f::Float64 + solverStatus + u + z + cost + MpcSol(a_x=0,d_f=0,solverStatus=0,u=0,z=0,cost=0) = new(a_x,d_f,solverStatus,u,z,cost) +end + +type TrackCoeff # coefficients of track + coeffAngle + coeffCurvature + nPolyCurvature # order of the interpolation polynom + width # lane width -> is used in cost function as soft constraints (to stay on track) + TrackCoeff(coeffAngle=0,coeffCurvature=0,nPolyCurvature=4) = new(coeffAngle,coeffCurvature,nPolyCurvature) +end + +type ModelParams + l_A::Float64 + l_B::Float64 + dt::Float64 + u_lb # lower bounds for u + u_ub # upper bounds + z_lb + z_ub + c0 + ModelParams(l_A=0.25,l_B=0.25,dt=0.1,u_lb=0,u_ub=0,z_lb=0,z_ub=0,c0=0) = new(l_A,l_B,dt,u_lb,u_ub,z_lb,z_ub,c0) +end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl new file mode 100644 index 00000000..0e9d3add --- /dev/null +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -0,0 +1,192 @@ +# This function evaluates and returns the coefficients for constraints and cost which are used later in the MPC formulation +# Inputs are: +# oldTraj -> contains information about previous trajectories and Inputs +# lapStatus -> contains information about the current lap +# mpcCoeff -> contains information about previous coefficient results +# posInfo -> contains information about the car's current position along the track +# mpcParams -> contains information about the MPC formulation (e.g. Q, R) +# stateIn -> current state of the car +# inputIn -> last input to the system + +# structure of oldTrajectory: 1st dimension = state number, 2nd dimension = step number (time equiv.), 3rd dimennsion = lap number + +function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) + # this computes the coefficients for the cost and constraints + + # Outputs: + # coeffConst + # coeffCost + + # Read Inputs + oldTrajectory = oldTraj.oldTraj # [:,:,1] = 1st, [:,:,2] = 2nd + oldInput = oldTraj.oldInput + + lapNumber = lapStatus.currentLap + + s_start = posInfo.s_start + s = posInfo.s + s_target = posInfo.s_target + + + # Parameters + N = mpcParams.N + nz = mpcParams.nz + R = mpcParams.R + Order = mpcCoeff.order # interpolation order for cost and constraints + + pLength = mpcCoeff.pLength # interpolation length for polynomials + + coeffCost = zeros(Order+1,1,2) # polynomial coefficients for cost + coeffConst = zeros(nz-1,Order+1,1,2) # nz-1 beacuse no coeff for s + + # Select the old data + oldS = oldTrajectory[1,:,:] + oldeY = oldTrajectory[2,:,:] + oldePsi = oldTrajectory[3,:,:] + oldV = oldTrajectory[4,:,:] + + N_points = size(oldTrajectory,2) # second dimension = length + + if lapNumber > 1 + deltaOld = oldInput[1,:,:] + aOld = oldInput[2,:,:] + + # Compute the total s (current position along track) + s_total = s_start + s + + # Compute the index + DistS = ( s_total - oldS ).^2 + + idx_s = findmin(DistS,2)[2] # contains both indices for the closest distances for both oldS !! + + vec_range = (idx_s[1]:min(idx_s[1]+pLength,N_points),idx_s[2]:min(idx_s[2]+pLength,N_points*2)) + if idx_s[1]+pLength > N_points || idx_s[2]+pLength > 2*N_points + warn("Out of range!") + println("vec_range = $vec_range") + println("idx_s = $idx_s") + println("s_total = $s_total") + end + + # Create the vectors used for the interpolation + # **************** WHAT IF MINIMUM INDEX + PLENGTH IS LONGER THAN ENTIRE OLD TRAJECTORY ? ******************************* + # -> oldTrajectory is designed to go way beyond the "real" measured limit + bS_Vector = cat(3, oldS[vec_range[1]], oldS[vec_range[2]]) + beY_Vector = cat(3, oldeY[vec_range[1]], oldeY[vec_range[2]]) + bePsi_Vector = cat(3, oldePsi[vec_range[1]], oldePsi[vec_range[2]]) + bV_Vector = cat(3, oldV[vec_range[1]], oldV[vec_range[2]]) + # These matrices (above) contain two vectors each (for both old trajectories), stored in the 3rd dimension + + # The states are parametrized with resprect to the curvilinear abscissa, + # so we select the point used for the interpolation. Need to subtract an + # offset to be coherent with the MPC formulation + s_forinterpy = bS_Vector - s_start + # Create the Matrices for the interpolation + MatrixInterp = zeros(pLength+1,Order+1,2) + + for k = 0:Order + MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,1,:].^k + end + + # Compute the coefficients + CoefficientsFor_ey = zeros(Order+1,1,2) + CoefficientsFor_ePsi = zeros(Order+1,1,2) + CoefficientsFor_V = zeros(Order+1,1,2) + for i=1:2 + CoefficientsFor_ey[:,:,i] = MatrixInterp[:,:,i]\beY_Vector[:,:,i] + CoefficientsFor_ePsi[:,:,i] = MatrixInterp[:,:,i]\bePsi_Vector[:,:,i] + CoefficientsFor_V[:,:,i] = MatrixInterp[:,:,i]\bV_Vector[:,:,i] + end + + # Stack the coefficients + coeffConst = zeros(3,Order+1,1,2) + coeffConst[1,:,:,:] = CoefficientsFor_ey + coeffConst[2,:,:,:] = CoefficientsFor_ePsi + coeffConst[3,:,:,:] = CoefficientsFor_V + # structure of coeffConst: + # 1st dimension specifies state + # 2nd dimension specifies steps + # 3rd dimension not used + # 4th dimension specifies lapNumber + + # Finished with calculating the constraint coefficients + + # Now compute the final cost coefficients + + # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value + # These values are calculated for both old trajectories + # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line + # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost + for i=1:2 + Qfunction = zeros(N_points,1) + IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position + idx_s_target = find(oldS[1,:,i].>s_target)[1] + dist_to_s_target = (idx_s_target - IndexBezierS) + dist_to_s_target = dist_to_s_target + 30 # set distance to finish line a bit higher + # for k=1:N_points + # # Start from the end ---> reverse the index + # indx = N_points-k+1 + + # # Here the if for the cost, minimize the distance to a straight line + # if oldS[1,indx,i] <= s_target + # QMatrix = 0.1 + # else + # QMatrix = 0 + # end + + # # Here actually computing the cost + # if (indx >= IndexBezierS) + # if k == 1 + # # If last point --> No Input + # Qfunction[indx] = QMatrix # [1] to transform 1-element-array to scalar + # else + # Qfunction[indx] = Qfunction[indx + 1] + QMatrix + # end + # end + # end + # NEEDS TO BE IMPROVED ************************** + bQfunction_Vector = zeros(pLength+1,1) + # Select the part needed for the interpolation + #bQfunction_Vector = Qfunction[IndexBezierS:IndexBezierS+pLength] + qLength = min(dist_to_s_target,pLength+1) + #println(bQfunction_Vector) + bQfunction_Vector = zeros(pLength+1,1) + bQfunction_Vector[1:qLength] = (dist_to_s_target:-1:dist_to_s_target-qLength+1)*0.1 + + #bQfunction_Vector = collect((dist_to_s_target:-1:dist_to_s_target-pLength))*0.1 + #println("length = $(length(bQfunction_Vector)), $(pLength+1)") + #println(bQfunction_Vector) + #readline() + + # Compute coefficient for the cost + coeffCost[:,1,i] = MatrixInterp[:,:,i]\bQfunction_Vector + # if maximum(coeffCost) > 1e4 + # warn("Large coefficients in cost, might cause numerical problems.") + # s = s_forinterpy[:,1,i] + # interp = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * coeffCost[:,1,i] + # println(coeffCost) + # plot(s,bQfunction_Vector,"-o",s,interp) + # grid() + # readline() + # end + end + + # if maximum(coeffConst) > 1e4 + # warn("Large coefficients in constraints, might cause numerical problems.") + # println(coeffConst) + # subplot(311) + # plot(s_forinterpy[:,:,1],beY_Vector[:,:,1],"-o") + # subplot(312) + # plot(s_forinterpy[:,:,1],bePsi_Vector[:,:,1],"-o") + # subplot(313) + # plot(s_forinterpy[:,:,1],bV_Vector[:,:,1],"-o") + # grid() + # readline() + # end + + + else # if it is the first lap + coeffCost = zeros(Order+1,1,2) + coeffConst = zeros(nz-1,Order+1,1,2) # nz-1 because no coeff for s + end + return MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) +end diff --git a/workspace/src/barc/src/helper/computeCostLap.jl b/workspace/src/barc/src/helper/computeCostLap.jl new file mode 100644 index 00000000..85d9325a --- /dev/null +++ b/workspace/src/barc/src/helper/computeCostLap.jl @@ -0,0 +1,9 @@ +function computeCostLap(z,s_target) + + f = find(z[:,1].>=s_target) + cost = 100000 + if size(f,1)>0 + cost = f[1] + end + return cost +end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/createModel.jl b/workspace/src/barc/src/helper/createModel.jl new file mode 100644 index 00000000..b501b9a1 --- /dev/null +++ b/workspace/src/barc/src/helper/createModel.jl @@ -0,0 +1,99 @@ +# Define Variables +oldTraj = OldTrajectory() +lapStatus = LapStatus(1,1) +mpcCoeff = MpcCoeff() +posInfo = PosInfo() +mpcParams = MpcParams() +stateIn = zeros(7,1) # stateIn: xdot, ydot, psidot, epsi, y, s, a_f +inputIn = zeros(2,1) +mpcSol = MpcSol() +trackCoeff = TrackCoeff() # info about track (at current position, approximated) +modelParams = ModelParams() + +# =============================== +# Initialize Parameters +# =============================== +buffersize = 700 +oldTraj.oldTraj = zeros(4,buffersize,2) +oldTraj.oldInput = zeros(2,buffersize,2) + +posInfo.s_start = 0 +posInfo.s_target = 2 + +mpcParams.N = 5 +mpcParams.nz = 4 +mpcParams.Q = [0.0 1.0 1.0 1.0] # put weights on ey, epsi and v +mpcParams.vPathFollowing = 0.2 + +mpcCoeff.coeffCost = 0 +mpcCoeff.coeffConst = 0 +mpcCoeff.order = 5 +mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon + +trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) +trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation +trackCoeff.width = 0.4 # width of the track (0.5m) + +modelParams.u_lb = [-2.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering +modelParams.u_ub = [2.0 pi/6]' * ones(1,mpcParams.N) # upper bounds +modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states +modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds +modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) +modelParams.l_A = 0.25 +modelParams.l_B = 0.25 +modelParams.dt = 0.1 + +z_Init = zeros(4,1) + +# ================================= +# Create Model +# ================================= +println("Building model...") +dt = modelParams.dt +L_a = modelParams.l_A +L_b = modelParams.l_B +N = mpcParams.N +c0 = modelParams.c0 + +mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.01))#,linear_solver="ma57",print_user_options="yes")) + +#@variable( mdl, modelParams.z_ub[i,j] >= z_Ol[i=1:4,j=1:(N+1)] >= modelParams.z_lb[i,j]) # z = s, ey, epsi, v +@variable( mdl, z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v +#@variable( mdl, modelParams.u_ub[i,j] >= u_Ol[i=1:2,j=1:N] >= modelParams.u_lb[i,j]) # u = a, d_f +@variable( mdl, u_Ol[1:2,1:N]) + +for i=1:2 # I don't know why but somehow the short method returns errors sometimes + for j=1:N + setlowerbound(u_Ol[i,j], modelParams.u_lb[i,j]) + setupperbound(u_Ol[i,j], modelParams.u_ub[i,j]) + end +end +#= +for i=1:4 + for j=1:N+1 + setlowerbound(z_Ol[i,j], modelParams.z_lb[i,j]) + setupperbound(z_Ol[i,j], modelParams.z_ub[i,j]) + end +end +=# + +#@constraint(mdl, [i=1:2,j=1:N], u_Ol[i,j] <= modelParams.u_ub[i,j]) +@variable( mdl, 1 >= ParInt >= 0 ) + +@NLparameter(mdl, z0[i=1:4] == z_Init[i]) +@NLconstraint(mdl, [i=1:4], z_Ol[i,1] == z0[i]) + +@NLparameter(mdl, coeff[i=1:length(trackCoeff.coeffCurvature)] == trackCoeff.coeffCurvature[i]); + +@NLexpression(mdl, c[i = 1:N], coeff[1]*z_Ol[1,i]^4+coeff[2]*z_Ol[1,i]^3+coeff[3]*z_Ol[1,i]^2+coeff[4]*z_Ol[1,i]+coeff[5]) +#@NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[1,i]^(5-j),j=1:5}) # gives segmentation fault (probably doesn't like x^0 = 1) +@NLexpression(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( c0[3]*u_Ol[2,i] + c0[4]*abs(u_Ol[2,i])*u_Ol[2,i]) ) ) +@NLexpression(mdl, dsdt[i = 1:N], z_Ol[4,i]*cos(z_Ol[3,i]+bta[i])/(1-z_Ol[2,i]*c[i])) + +# System dynamics +for i=1:N + @NLconstraint(mdl, z_Ol[1,i+1] == z_Ol[1,i] + dt*dsdt[i] ) # s + @NLconstraint(mdl, z_Ol[2,i+1] == z_Ol[2,i] + dt*z_Ol[4,i]*sin(z_Ol[3,i]+bta[i]) ) # ey + @NLconstraint(mdl, z_Ol[3,i+1] == z_Ol[3,i] + dt*(z_Ol[4,i]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) # epsi + @NLconstraint(mdl, z_Ol[4,i+1] == z_Ol[4,i] + dt*(c0[1]*u_Ol[1,i] - c0[2]*abs(z_Ol[4,i]) * z_Ol[4,i])) # v +end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl new file mode 100644 index 00000000..b40225c0 --- /dev/null +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -0,0 +1,133 @@ +# Variable definitions +# z_Ol[i,j] = z_OpenLoop, open loop prediction of the state, i = state, j = step + +# States: +# i = 1 -> s +# i = 2 -> ey +# i = 3 -> epsi +# i = 4 -> v + +function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr,uCurr) + + # Load Parameters + coeffCurvature = trackCoeff.coeffCurvature + N = mpcParams.N + Q = mpcParams.Q + R = mpcParams.R + coeffTermCost = mpcCoeff.coeffCost + coeffTermConst = mpcCoeff.coeffConst + order = mpcCoeff.order # polynomial order of terminal constraints and cost approximation + s_start = posInfo.s_start + s_target = posInfo.s_target + ey_max = trackCoeff.width/2 + + v_ref = mpcParams.vPathFollowing + if lapStatus.currentLap > 1 + #v_ref = 10 + end + + global mdl + + # Create function-specific parameters + z_Ref = cat(1,s_target*ones(1,N+1),zeros(2,N+1),v_ref*ones(1,N+1)) # Reference trajectory: path following -> stay on line and keep constant velocity + u_Ref = zeros(2,N) + + # Update current initial condition + setvalue(z0,zCurr) + # Update curvature + setvalue(coeff,coeffCurvature) + + @NLexpression(mdl, costZ, 0) + @NLexpression(mdl, costZTerm, 0) + @NLexpression(mdl, constZTerm, 0) + @NLexpression(mdl, derivCost, 0) + @NLexpression(mdl, laneCost, 0) + + # Derivative cost + # --------------------------------- + @NLexpression(mdl, derivCost, 0*sum{QderivZ[j]*((zCurr[j]-z_Ol[j,1])^2+sum{(z_Ol[j,i]-z_Ol[j,i+1])^2,i=1:N}),j=1:4} + + sum{QderivU[j]*((uCurr[j]-u_Ol[j,1])^2+sum{(u_Ol[j,i]-u_Ol[j,i+1])^2,i=1:N-1}),j=1:2}) + + # Lane cost + # --------------------------------- + @NLexpression(mdl, laneCost, 10*sum{(0.5+0.5*tanh(50*(z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(z_Ol[2,i]+ey_max))),i=1:N+1}) + + # Control Input cost + # --------------------------------- + @NLexpression(mdl, controlCost, 0.5*sum{R[j]*sum{(u_Ol[j,i]-u_Ref[j,i])^2,i=1:N},j=1:2}) + + # Terminal constraints (soft), starting from 2nd lap + # --------------------------------- + if lapStatus.currentLap > 2 # if at least in the 3rd lap + # termStateErr = (ParInt*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + + # + (1 - ParInt)*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; + @NLexpression(mdl, constZTerm, 10*(sum{(ParInt*sum{coeffTermConst[j,i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermConst[j,i,1,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3})) + elseif lapStatus.currentLap == 2 # if in the 2nd lap + # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; + @NLexpression(mdl, constZTerm, 10*sum{(sum{coeffTermConst[j,i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3}) + end + + # Terminal cost + # --------------------------------- + if zCurr[1] + s_start > s_target # if finish line crossed + println("Crossed finish line. I should not be here.") + @NLexpression(mdl, costZTerm, 0) # terminal cost is zero + else + if lapStatus.currentLap > 2 # if at least in the 3rd lap + # costZTerm = ParInt*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + + # + (1-ParInt)*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); + @NLexpression(mdl, costZTerm, ParInt*sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermCost[i,1,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) + # line above not necessary since the polynomial goes to zero anyways! + elseif lapStatus.currentLap == 2 # if we're in the second second lap + # costZTerm = polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); + @NLexpression(mdl, costZTerm, sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) + end + end + + # State cost + # --------------------------------- + if lapStatus.currentLap == 1 || zCurr[1] + s_start > s_target # if we're in the first lap or crossed finish line, just do path following + # currCostZ = 0.5*Q[j]*pow(ZOlGlobal[(i+1)*nz+j]-ZRefGlobal[(i+1)*nz+j],2); + if lapStatus.currentLap > 1 + println("I should not be here.") + end + @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[i,j]-z_Ref[i,j])^2,j=1:N+1},i=1:4}) # Follow trajectory + #println(z_Ref) + else + #@NLexpression(mdl, costZ_h, 0) # zero state cost after crossing the finish line + #@NLexpression(mdl, costZ, 1 + (costZ_h-1) * (0.5+0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) + @NLexpression(mdl, costZ, 1) + #@NLexpression(mdl, costZ, 1) + end + + @NLobjective(mdl, Min, costZ + costZTerm + constZTerm + derivCost + controlCost + laneCost) + + # Solve Problem and return solution + sol_status = solve(mdl) + sol_u = getvalue(u_Ol) + mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(u_Ol),getvalue(z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) + #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging + #println(getvalue(costZTerm)) + #println(getvalue(z_Ol[1,N+1])) + #println(getvalue(constZTerm)) + #println("ParInt = $(getvalue(ParInt))") + #println("u = $(sol_u[:,1])") + + # if lapStatus.currentLap > 100 + # ss = collect(zCurr[1]:.01:zCurr[1]+0.3) + # #p = [ss.^4 ss.^3 ss.^2 ss.^1 ss.^0] *coeffTermCost[:,:,1] + # p = [ss.^4 ss.^3 ss.^2 ss.^1 ss.^0] *coeffTermConst[:,:,1,1] + # plot(ss,p,getvalue(z_Ol[1,N+1]),getvalue(costZTerm),"o") + # grid() + # readline() + # end + + # println(getvalue(z_Ol)) + # println("==============") + # println(getvalue(u_Ol)) + return mpcSol +end From 291dde60fc50a3b3f558bfab70f3a8210b28e44b Mon Sep 17 00:00:00 2001 From: maxb91 Date: Tue, 13 Sep 2016 09:02:22 -0700 Subject: [PATCH 005/183] Added indoor GPS package --- .../src/marvelmind_indoor_gps/CMakeLists.txt | 187 ++++++++++++++++++ .../launch/indoor_gps.launch | 6 + .../src/marvelmind_indoor_gps/package.xml | 14 ++ .../marvelmind_indoor_gps/src/indoor_gps.py | 79 ++++++++ 4 files changed, 286 insertions(+) create mode 100644 workspace/src/marvelmind_indoor_gps/CMakeLists.txt create mode 100644 workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch create mode 100644 workspace/src/marvelmind_indoor_gps/package.xml create mode 100755 workspace/src/marvelmind_indoor_gps/src/indoor_gps.py diff --git a/workspace/src/marvelmind_indoor_gps/CMakeLists.txt b/workspace/src/marvelmind_indoor_gps/CMakeLists.txt new file mode 100644 index 00000000..9329559e --- /dev/null +++ b/workspace/src/marvelmind_indoor_gps/CMakeLists.txt @@ -0,0 +1,187 @@ +cmake_minimum_required(VERSION 2.8.3) +project(marvelmind_indoor_gps) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + std_msgs + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES marvelmind_indoor_gps +# CATKIN_DEPENDS rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(marvelmind_indoor_gps +# src/${PROJECT_NAME}/marvelmind_indoor_gps.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(marvelmind_indoor_gps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(marvelmind_indoor_gps_node src/marvelmind_indoor_gps_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(marvelmind_indoor_gps_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(marvelmind_indoor_gps_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS marvelmind_indoor_gps marvelmind_indoor_gps_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_marvelmind_indoor_gps.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch b/workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch new file mode 100644 index 00000000..5991fae4 --- /dev/null +++ b/workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/workspace/src/marvelmind_indoor_gps/package.xml b/workspace/src/marvelmind_indoor_gps/package.xml new file mode 100644 index 00000000..8dd95296 --- /dev/null +++ b/workspace/src/marvelmind_indoor_gps/package.xml @@ -0,0 +1,14 @@ + + + marvelmind_indoor_gps + 0.0.0 + The marvelmind_indoor_gps package + odroid + MIT + catkin + rospy + rospy + + + + diff --git a/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py b/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py new file mode 100755 index 00000000..b36112e5 --- /dev/null +++ b/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python + +import rospy +import sys +from time import sleep +import serial +import struct +import time +from numpy import pi +from geometry_msgs.msg import Vector3 +import crcmod +import traceback + +def indoor_gps_init(serial_device, baudrate): + # open port + try: + ser = serial.Serial(serial_device, baudrate, timeout=3 ) + rospy.loginfo("opened port!") + except serial.serialutil.SerialException: + print 'Can not open serial port(%s)'%(serial_device) + traceback.print_exc() + return None + + # return serial port + ser.flushInput() + return ser + +def indoor_gps_data_acq(): + # start node + rospy.init_node('indoor_gps', anonymous=True) + pub = rospy.Publisher('indoor_gps', Vector3, queue_size = 10) + rospy.loginfo("started node!") + + # open port + port = rospy.get_param("indoor_gps/port") + baud = rospy.get_param("indoor_gps/baud") + ser = indoor_gps_init(port, baud) + + pktSize = 23 + + while not rospy.is_shutdown(): + ser = serial.Serial(port, baud, timeout=3 ) + ser.flushInput() + len_buf = pktSize*3 - 1 + buf = ser.read(len_buf) + buf = '' + + while buf is not None: + buf += ser.read(len_buf) + pktHdrOffset = buf.find('\xff\x47\x01\x00') + + while pktHdrOffset == -1: + ser.flushInput() + buf = ser.read(len_buf) + pktHdrOffset = buf.find('\xff\x47\x01\x00') + + if len_buf-pktHdrOffset-5>=18: + usnTimestamp, usnX, usnY, usnCRC16 = struct.unpack_from ( ' Date: Tue, 13 Sep 2016 09:07:00 -0700 Subject: [PATCH 006/183] Fixed localization node (fixed wrapping error) and added position information message --- workspace/src/barc/CMakeLists.txt | 1 + workspace/src/barc/msg/pos_info.msg | 9 +++++++++ workspace/src/barc/src/Localization_helpers.py | 15 ++++++++------- workspace/src/barc/src/Localization_node.py | 17 ++++++++--------- 4 files changed, 26 insertions(+), 16 deletions(-) create mode 100644 workspace/src/barc/msg/pos_info.msg diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index 366e2c82..dcb7bf59 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -51,6 +51,7 @@ add_message_files( Encoder.msg ECU.msg Z_KinBkMdl.msg + pos_info.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg new file mode 100644 index 00000000..f3ef6225 --- /dev/null +++ b/workspace/src/barc/msg/pos_info.msg @@ -0,0 +1,9 @@ +# This is a message to hold data with position and trajectory information +float32 s +float32 ey +float32 epsi +float32 v +float32[] coeffX +float32[] coeffY +float32[] coeffTheta +float32[] coeffCurvature diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index b22cb43b..d5bfba0f 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -202,13 +202,14 @@ def find_s(self): ey = eyabs*sign(sin(xyVectorAngle-xyPathAngle)) # Calculate epsi - epsi = self.psi-xyPathAngle - if abs(epsi) > pi/2: - if epsi < pi/2: - epsi = epsi + 2*pi - else: - epsi = epsi - 2*pi - + epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle + epsi = (epsi+pi)%(2*pi)-pi + #if abs(epsi) > pi/2: + # if epsi < pi/2: + # epsi = epsi + 2*pi + # else: + # epsi = epsi - 2*pi + #epsi = (self.psi+pi)%(2*pi)+pi-xyPathAngle self.epsi = epsi self.ey = ey self.s = s diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 2ed4c475..6ae492a4 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -17,15 +17,14 @@ # State estimate callback function def state_estimate_callback(data): - global l, epsi_prev - # Set current position and orientation - l.set_pos(data.x,data.y,data.psi,data.v) - # Update position and trajectory information - l.find_s() - + global l, epsi_prev + # Set current position and orientation + l.set_pos(data.x,data.y,data.psi,data.v) + # Update position and trajectory information + l.find_s() # unwrap epsi - l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] - epsi_prev = l.epsi; + # l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] + epsi_prev = l.epsi # localization node def localization_node(): @@ -41,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(2.5,1.5,0.2,array([3.5,1.0]),0) + l.create_racetrack(1.0,2.0,0.1,array([3.5,1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) From 05715bf2eb82f4d4f9af9603d4d1b594eb44c548 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Tue, 13 Sep 2016 09:19:44 -0700 Subject: [PATCH 007/183] Added MPC node for trajectory following and MPC-logging message format. Also added Launch-file and observers file for estimator --- workspace/src/barc/CMakeLists.txt | 1 + workspace/src/barc/launch/follow_traj.launch | 52 ++++++ workspace/src/barc/msg/Logging.msg | 17 ++ workspace/src/barc/src/controller_MPC_traj.jl | 157 ++++++++++++++++++ workspace/src/barc/src/observers.py | 124 ++++++++++++++ 5 files changed, 351 insertions(+) create mode 100644 workspace/src/barc/launch/follow_traj.launch create mode 100644 workspace/src/barc/msg/Logging.msg create mode 100755 workspace/src/barc/src/controller_MPC_traj.jl create mode 100755 workspace/src/barc/src/observers.py diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index dcb7bf59..c05c7b8c 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -51,6 +51,7 @@ add_message_files( Encoder.msg ECU.msg Z_KinBkMdl.msg + Logging.msg pos_info.msg ) diff --git a/workspace/src/barc/launch/follow_traj.launch b/workspace/src/barc/launch/follow_traj.launch new file mode 100644 index 00000000..9083988b --- /dev/null +++ b/workspace/src/barc/launch/follow_traj.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/msg/Logging.msg b/workspace/src/barc/msg/Logging.msg new file mode 100644 index 00000000..59cac00e --- /dev/null +++ b/workspace/src/barc/msg/Logging.msg @@ -0,0 +1,17 @@ +# This is a message to hold data for the ECU (electronic control unit) +# +# Units may vary depending on the topic +# The motor controls the speeds of the vehicle through an input torque. (For input force, divide by radius of tire) +# The servo controls the steering angle +# +# For modeling and state estimation, motors units are [N], and servo units are [rad] +# For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle +float32 obj_val +string status +float32 solve_time +float32[] s +float32[] ey +float32[] epsi +float32[] v +float32[] a +float32[] d_f \ No newline at end of file diff --git a/workspace/src/barc/src/controller_MPC_traj.jl b/workspace/src/barc/src/controller_MPC_traj.jl new file mode 100755 index 00000000..0ff74fb6 --- /dev/null +++ b/workspace/src/barc/src/controller_MPC_traj.jl @@ -0,0 +1,157 @@ +#!/usr/bin/env julia + +#= + Licensing Information: You are free to use or extend these projects for + education or reserach purposes provided that (1) you retain this notice + and (2) you provide clear attribution to UC Berkeley, including a link + to http://barc-project.com + + Attibution Information: The barc project ROS code-base was developed + at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales + (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed + by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was + based on an open source project by Bruce Wootton +=# + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using JuMP +using Ipopt + + +# define model parameters +L_a = 0.125 # distance from CoG to front axel +L_b = 0.125 # distance from CoG to rear axel +dt = 0.1 # time step of system +coeffCurvature = [0,0,0,0] + +# preview horizon +N = 15 + +# define targets [generic values] +v_ref = 0.8 + +# define objective function values +c_ey = 50 +c_ev = 10 +c_epsi = 5 +c_df = 0.3 +c_a = 0.2 +c_ey_f = 0 +c_ev_f = 0 +c_epsi_f = 0 + +# define decision defVars +# states: position (x,y), yaw angle, and velocity +# inputs: acceleration, steering angle +println("Creating kinematic bicycle model ....") +mdl = Model(solver = IpoptSolver(print_level=3,max_cpu_time=0.1,linear_solver="ma97",print_user_options="yes")) + +@defVar( mdl, s[1:(N+1)] ) +@defVar( mdl, ey[1:(N+1)] ) +@defVar( mdl, epsi[1:(N+1)] ) +@defVar( mdl, 0.0 <= v[1:(N+1)] <= 3.0 ) +@defVar( mdl, 2.0 >= a[1:N] >= -1.0 ) +@defVar( mdl, -0.6 <= d_f[1:N] <= 0.6 ) + +# define objective function +@setNLObjective(mdl, Min, sum{c_ey*ey[i]^2+c_ev*(v[i]-v_ref)^2+c_epsi*epsi[i]^2+c_df*d_f[i]^2+c_a*a[i]^2,i=1:N} + c_ey_f*ey[N+1]^2 + c_ev_f*(v[N+1]-v_ref)^2 + c_epsi_f*epsi[N+1]^2) + +# define constraints +# define system dynamics +# Reference: R.Rajamani, Vehicle Dynamics and Control, set. Mechanical Engineering Series, +# Spring, 2011, page 26 + +@defNLParam(mdl, s0 == 0); @addNLConstraint(mdl, s[1] == s0); +@defNLParam(mdl, ey0 == 0); @addNLConstraint(mdl, ey[1] == ey0); +@defNLParam(mdl, epsi0 == 0); @addNLConstraint(mdl, epsi[1] == epsi0 ); +@defNLParam(mdl, v0 == 0); @addNLConstraint(mdl, v[1] == v0); +@defNLParam(mdl, coeff[i=1:length(coeffCurvature)]==coeffCurvature[i]); +@defNLExpr(mdl, c[i = 1:N], coeff[1]*s[i]^3+coeff[2]*s[i]^2+coeff[3]*s[i]+coeff[4]) +@defNLExpr(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( d_f[i]) ) ) + +@defNLExpr(mdl, dsdt[i = 1:N], v[i]*cos(epsi[i]+bta[i])/(1-ey[i]*c[i])) +for i in 1:N + @addNLConstraint(mdl, s[i+1] == s[i] + dt*dsdt[i] ) + @addNLConstraint(mdl, ey[i+1] == ey[i] + dt*v[i]*sin(epsi[i]+bta[i]) ) + @addNLConstraint(mdl, epsi[i+1] == epsi[i] + dt*(v[i]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) + @addNLConstraint(mdl, v[i+1] == v[i] + dt*(a[i] - 0.63 *abs(v[i])*v[i]) ) +end + + +# status update +println("initial solve ...") +solve(mdl) +println("finished initial solve!") + + +# write info in logfile +currenttime = now() +f = open("/home/odroid/rosbag/MPClog_$currenttime.txt","w") +byteswritten=write(f,"MPC LOG, created $currenttime\n=========================\nv_ref = $v_ref\nN = $N\nc_ey = $c_ey\nc_ev = $c_ev\nc_epsi = $c_epsi\nc_df = $c_df\nc_a = $c_a\nc_ey_f = $c_ey_f\nc_ev_f = $c_ev_f\nc_epsi_f = $c_epsi_f") +close(f) + +if byteswritten>0 + println("Successfully written to Logfile MPClog_$currenttime.txt") +else + println("Problem with Logfile") +end + + +function SE_callback(msg::pos_info) + # update mpc initial condition + setValue(s0, msg.s) + setValue(ey0, msg.ey) + setValue(epsi0, msg.epsi) + setValue(v0, msg.v) + setValue(coeff, msg.coeffCurvature) +end + +function main() + # initiate node, set up publisher / subscriber topics + init_node("mpc_traj") + pub = Publisher("ecu", ECU, queue_size=10) + pub2 = Publisher("logging", Logging, queue_size=10) + s1 = Subscriber("pos_info", pos_info, SE_callback, queue_size=10) + loop_rate = Rate(10) + cmdcount = 0 + failcount = 0 + while ! is_shutdown() + # run mpc, publish command + tic() + status = solve(mdl) + solvetime = toc() + if status == Symbol("Optimal") + # get optimal solutions + a_opt = getValue(a[1]) + d_f_opt = getValue(d_f[1]) + cmd = ECU(a_opt, d_f_opt) + # publish commands + if cmdcount>10 # ignore first 10 commands since MPC often stagnates during the first seconds (why?) + publish(pub, cmd) + end + failcount = 0 + else + failcount = failcount + 1 + if failcount >= 5 # if at least 5 unsolved problems in a row + cmd = ECU(0,0) # stop car + publish(pub,cmd) + end + end + println("Solve Status: ", string(status), "\nSolve Time: ", solvetime,"\n\n") + loginfo = Logging(getObjectiveValue(mdl),string(status),solvetime,getValue(s),getValue(ey),getValue(epsi),getValue(v),getValue(a),getValue(d_f)) + publish(pub2, loginfo) + cmdcount = cmdcount + 1 + rossleep(loop_rate) + end +end + +if ! isinteractive() + main() +end diff --git a/workspace/src/barc/src/observers.py b/workspace/src/barc/src/observers.py new file mode 100755 index 00000000..3da79a87 --- /dev/null +++ b/workspace/src/barc/src/observers.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +from numpy import array, dot, eye, copy +from numpy import dot, zeros +from scipy.linalg import inv +import rospy + +C = array([[1, 0]]) +B = eye(2) +def kinematicLuembergerObserver(vhat_x, vhat_y, w_z, a_x, a_y, v_x_enc, aph, dt): + """ + inputs: + * current longitudinal velocity estimate vhat_x [m/s] + * current lateral velocity estimate vhat_y [m/s] + * yaw rate measurement from gyroscope [rad/s] + * a_x measurement from IMU [m/s^2] + * a_y measurement from IMU [m/s^2] + * v_x estimate from encoder (v_x_enc) [m/s] + output: + * longitudinal velocity estimate vhat_x at next time step [m/s] + * lateral velocity estimate vhat_y at next time step [m/s] + + reference: + Farrelly, Jim, and Peter Wellstead. "Estimation of vehicle lateral velocity." + Control Applications, 1996. Proceedings of the 1996 IEEE International Conference + on IEEE, 1996 + Equations (25) - (31) + """ + + # compute the observer gain + # note, the reshape(-1,1) transforms the array into a n x 1 vector + K = array([ 2*aph*abs(w_z), (aph**2 - 1)*w_z ]).reshape(-1,1) + + # if car is not moving, then acclerations should be zero + if v_x_enc == 0: + a_x = 0 + a_y = 0 + vhat_x = 0 + vhat_y = 0 + + # build system matrices + A = array([[ 0, w_z], \ + [-w_z, 0 ]]) + u = array([a_x, a_y]).reshape(-1,1) + vhat_k = array([vhat_x, vhat_y]).reshape(-1,1) + + # apply observer + vhat_kp1 = vhat_k + dt*( dot( (A - dot(K,C)), vhat_k) + dot(B,u) + K*v_x_enc) + + return vhat_kp1 + + +def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + mx_k: "a priori" state estimate + P_k: "a priori" estimated state covariance + h: fanction handle for h(x) + y_kp1: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + + xDim = mx_k.size # dimension of the state + mx_kp1 = f(mx_k, *args) # predict next state + A = numerical_jac(f, mx_k, *args) # linearize process model about current state + P_kp1 = dot(dot(A,P_k),A.T) + Q # proprogate variance + my_kp1 = h(mx_kp1) # predict future output + H = numerical_jac(h, mx_kp1) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + mx_kp1 = mx_kp1 + dot(K,(y_kp1 - my_kp1)) # state estimate + P_kp1 = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + return (mx_kp1, P_kp1) + + + +def numerical_jac(f,x, *args): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = f(x, *args) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = f(xp, *args) + xp[i] = x[i] - eps/2.0 + ylo = f(xp, *args) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac From 002eed5e0898d8401d3ee4766babd2bcc97f1840 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 13 Sep 2016 09:31:37 -0700 Subject: [PATCH 008/183] added observers file (necessary for state estimation) --- workspace/src/barc/src/observers.py | 124 ++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100755 workspace/src/barc/src/observers.py diff --git a/workspace/src/barc/src/observers.py b/workspace/src/barc/src/observers.py new file mode 100755 index 00000000..3da79a87 --- /dev/null +++ b/workspace/src/barc/src/observers.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +from numpy import array, dot, eye, copy +from numpy import dot, zeros +from scipy.linalg import inv +import rospy + +C = array([[1, 0]]) +B = eye(2) +def kinematicLuembergerObserver(vhat_x, vhat_y, w_z, a_x, a_y, v_x_enc, aph, dt): + """ + inputs: + * current longitudinal velocity estimate vhat_x [m/s] + * current lateral velocity estimate vhat_y [m/s] + * yaw rate measurement from gyroscope [rad/s] + * a_x measurement from IMU [m/s^2] + * a_y measurement from IMU [m/s^2] + * v_x estimate from encoder (v_x_enc) [m/s] + output: + * longitudinal velocity estimate vhat_x at next time step [m/s] + * lateral velocity estimate vhat_y at next time step [m/s] + + reference: + Farrelly, Jim, and Peter Wellstead. "Estimation of vehicle lateral velocity." + Control Applications, 1996. Proceedings of the 1996 IEEE International Conference + on IEEE, 1996 + Equations (25) - (31) + """ + + # compute the observer gain + # note, the reshape(-1,1) transforms the array into a n x 1 vector + K = array([ 2*aph*abs(w_z), (aph**2 - 1)*w_z ]).reshape(-1,1) + + # if car is not moving, then acclerations should be zero + if v_x_enc == 0: + a_x = 0 + a_y = 0 + vhat_x = 0 + vhat_y = 0 + + # build system matrices + A = array([[ 0, w_z], \ + [-w_z, 0 ]]) + u = array([a_x, a_y]).reshape(-1,1) + vhat_k = array([vhat_x, vhat_y]).reshape(-1,1) + + # apply observer + vhat_kp1 = vhat_k + dt*( dot( (A - dot(K,C)), vhat_k) + dot(B,u) + K*v_x_enc) + + return vhat_kp1 + + +def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + mx_k: "a priori" state estimate + P_k: "a priori" estimated state covariance + h: fanction handle for h(x) + y_kp1: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + + xDim = mx_k.size # dimension of the state + mx_kp1 = f(mx_k, *args) # predict next state + A = numerical_jac(f, mx_k, *args) # linearize process model about current state + P_kp1 = dot(dot(A,P_k),A.T) + Q # proprogate variance + my_kp1 = h(mx_kp1) # predict future output + H = numerical_jac(h, mx_kp1) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + mx_kp1 = mx_kp1 + dot(K,(y_kp1 - my_kp1)) # state estimate + P_kp1 = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + return (mx_kp1, P_kp1) + + + +def numerical_jac(f,x, *args): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = f(x, *args) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = f(xp, *args) + xp[i] = x[i] - eps/2.0 + ylo = f(xp, *args) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac From 526ffbee294e52b8c545eae1d8f7603e18469f6d Mon Sep 17 00:00:00 2001 From: maxb91 Date: Tue, 13 Sep 2016 14:49:57 -0700 Subject: [PATCH 009/183] Added Barc simulation node, still needs launch-file to be started (together with according control) --- workspace/src/barc/src/barc_simulator.jl | 128 +++++++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 workspace/src/barc/src/barc_simulator.jl diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl new file mode 100644 index 00000000..77715b00 --- /dev/null +++ b/workspace/src/barc/src/barc_simulator.jl @@ -0,0 +1,128 @@ +#!/usr/bin/env julia + +#= + Licensing Information: You are free to use or extend these projects for + education or reserach purposes provided that (1) you retain this notice + and (2) you provide clear attribution to UC Berkeley, including a link + to http://barc-project.com + + Attibution Information: The barc project ROS code-base was developed + at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales + (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed + by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was + based on an open source project by Bruce Wootton +=# + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using sensor_msgs.msg + +function simModel(z,u,dt,l_A,l_B) + + bta = atan(L_a/(L_a+L_b)*tan(u[2])) + + zNext = z + zNext[1] = z[1] + dt*(z[4]*cos(z[3]+bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/L_b*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1]) # v + + return zNext +end + + +# define model parameters +L_a = 0.125 # distance from CoG to front axel +L_b = 0.125 # distance from CoG to rear axel + +u_current = zeros(2,1) + +function ECU_callback(msg::ECU) + + global u_current + u_current = [msg.motor, msg.servo] +end + +function main() + # initiate node, set up publisher / subscriber topics + init_node("barc_sim") + pub_enc = Publisher("encoder", Encoder, queue_size=10) + pub_gps = Publisher("indoor_gps", Vector3, queue_size=10) + pub_imu = Publisher("imu/data", Imu, queue_size=10) + + l_A = get_param("L_a") # distance from CoG to front axel + l_B = get_param("L_b") # distance from CoG to rear axel + + s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) + + z_current = zeros(60000,4) + + dt = 0.01 + loop_rate = Rate(1/dt) + + i = 2 + + dist_traveled = 0 + last_updated = 0 + + r_tire = 0.036 # radius from tire center to perimeter along magnets [m] + quarterCirc = 0.5 * pi * r_tire + + FL = 0 + FR = 0 + BL = 0 + BR = 0 + + println("Publishing sensor information. Simulator running.") + while ! is_shutdown() + + # Simulate state + z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' + dist_traveled += z_current[i,4]*dt + + # Encoder measurements + if dist_traveled - last_updated >= quarterCirc + last_updated = dist_traveled + FL += 1 + FR += 1 + BL += 1 + BR += 0 + enc_data = Encoder(FL, FR, BL, BR) + publish(pub_enc, enc_data) + end + + # IMU measurements + imu_data = Imu() + imu_data.orientation = geometry_msgs.msg.Quaternion(cos(z_current[i,3]/2), sin(z_current[i,3]/2), 0, 0) + if i%2 == 0 + publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" + # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) + end + + # GPS measurements + x = z_current[i,1]*100 # Indoor gps measures in cm + y = z_current[i,2]*100 + if i % 14 == 0 + gps_data = Vector3(x,y,0) + publish(pub_gps, gps_data) + end + + i += 1 + rossleep(loop_rate) + end + + # Save simulation data to file + println("Exiting node... Saving data. Simulated $((i-1)*dt) seconds.") + writedlm("/home/test.txt",z_current[1:i-1,:]) +end + +if ! isinteractive() + main() +end \ No newline at end of file From adfbaaf8f7dfdc1e557a058718e2036b529a9194 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Tue, 13 Sep 2016 14:59:59 -0700 Subject: [PATCH 010/183] added launch file for MPC-follow-trajectory simulation --- workspace/src/barc/launch/barc_sim.launch | 36 +++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 workspace/src/barc/launch/barc_sim.launch diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch new file mode 100644 index 00000000..e9b6ab5d --- /dev/null +++ b/workspace/src/barc/launch/barc_sim.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From d23fefa7efd9e2184e3af87799ee1f51e1c2e383 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 13 Sep 2016 16:29:47 -0700 Subject: [PATCH 011/183] deleted outlier detection in state estimator (didn't work properly) and did little corrections in the simulation node --- workspace/src/barc/launch/barc_sim.launch | 6 ++++-- workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 15 ++++++++------- workspace/src/barc/src/controller_MPC_traj.jl | 4 ++-- .../src/barc/src/state_estimation_KinBkMdl.py | 18 ++---------------- 5 files changed, 17 insertions(+), 28 deletions(-) mode change 100644 => 100755 workspace/src/barc/src/barc_simulator.jl diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index e9b6ab5d..4423ba82 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -14,13 +14,15 @@ - + - + + + diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 6ae492a4..338c3682 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(1.0,2.0,0.1,array([3.5,1.0]),0) + l.create_racetrack(2.0,2.0,0.1,array([0.0,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl old mode 100644 new mode 100755 index 77715b00..283d7e75 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -31,8 +31,8 @@ function simModel(z,u,dt,l_A,l_B) zNext = z zNext[1] = z[1] + dt*(z[4]*cos(z[3]+bta)) # x zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/L_b*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1]) # v + zNext[3] = z[3] + dt*(z[4]/L_b*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63*z[4]^2 * sign(z[4])) # v return zNext end @@ -87,12 +87,12 @@ function main() z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' dist_traveled += z_current[i,4]*dt - # Encoder measurements + # Encoder measurements, might be adapted according to the number of encoders on the car if dist_traveled - last_updated >= quarterCirc last_updated = dist_traveled FL += 1 FR += 1 - BL += 1 + BL += 0 BR += 0 enc_data = Encoder(FL, FR, BL, BR) publish(pub_enc, enc_data) @@ -119,10 +119,11 @@ function main() end # Save simulation data to file - println("Exiting node... Saving data. Simulated $((i-1)*dt) seconds.") - writedlm("/home/test.txt",z_current[1:i-1,:]) + log_path = "$(homedir())/simulations/output.txt" + println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") + writedlm(log_path,z_current[1:i-1,:]) end if ! isinteractive() main() -end \ No newline at end of file +end diff --git a/workspace/src/barc/src/controller_MPC_traj.jl b/workspace/src/barc/src/controller_MPC_traj.jl index 0ff74fb6..db1f7c64 100755 --- a/workspace/src/barc/src/controller_MPC_traj.jl +++ b/workspace/src/barc/src/controller_MPC_traj.jl @@ -32,7 +32,7 @@ dt = 0.1 # time step of system coeffCurvature = [0,0,0,0] # preview horizon -N = 15 +N = 10 # define targets [generic values] v_ref = 0.8 @@ -51,7 +51,7 @@ c_epsi_f = 0 # states: position (x,y), yaw angle, and velocity # inputs: acceleration, steering angle println("Creating kinematic bicycle model ....") -mdl = Model(solver = IpoptSolver(print_level=3,max_cpu_time=0.1,linear_solver="ma97",print_user_options="yes")) +mdl = Model(solver = IpoptSolver(print_level=3,max_cpu_time=0.1)) # ,linear_solver="ma97",print_user_options="yes")) @defVar( mdl, s[1:(N+1)] ) @defVar( mdl, ey[1:(N+1)] ) diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py index 0f076df9..6c9bce79 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl.py @@ -199,9 +199,8 @@ def state_estimation(): else: rospy.logerr("No estimation mode selected.") - count = 0 - x_prev = 0 - y_prev = 0 + x_meas = 0 + y_meas = 0 # start loop while not rospy.is_shutdown(): @@ -213,15 +212,6 @@ def state_estimation(): state_pub.publish( Z_KinBkMdl(x_e, y_e, psi_e, v_e) ) # collect measurements, inputs, system properties - - # check gps values for *single* outlier - if count > 100: # only if estimation is already about right - if est_mode == 1 or est_mode == 3: # if gps is included in the measurements - gps_diff = array([x_meas-x_e,y_meas-y_e]) # vector difference between estimate and gps position - if (gps_diff[0]**2+gps_diff[1]**2)**0.5 > 0.8: # outlier distance: 0.8m - x_meas = x_prev - y_meas = y_prev - if est_mode==1: y = array([x_meas, y_meas, psi_meas, v_meas]) elif est_mode==2: @@ -235,10 +225,6 @@ def state_estimation(): # apply EKF and get each state estimate (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) - x_prev = x_meas - y_prev = y_meas - count = count + 1 - # wait rate.sleep() From 574decb64a37d2b92418440d8fcbcdda41ee63ee Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 14 Sep 2016 09:12:54 -0700 Subject: [PATCH 012/183] changed paths of log files to make them platform-independent --- workspace/src/barc/src/controller_MPC_traj.jl | 2 +- workspace/src/barc/src/state_estimation_KinBkMdl.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/workspace/src/barc/src/controller_MPC_traj.jl b/workspace/src/barc/src/controller_MPC_traj.jl index db1f7c64..9d9f7c5a 100755 --- a/workspace/src/barc/src/controller_MPC_traj.jl +++ b/workspace/src/barc/src/controller_MPC_traj.jl @@ -93,7 +93,7 @@ println("finished initial solve!") # write info in logfile currenttime = now() -f = open("/home/odroid/rosbag/MPClog_$currenttime.txt","w") +f = open("$(homedir())/rosbag/MPClog_$currenttime.txt","w") byteswritten=write(f,"MPC LOG, created $currenttime\n=========================\nv_ref = $v_ref\nN = $N\nc_ey = $c_ey\nc_ev = $c_ev\nc_epsi = $c_epsi\nc_df = $c_df\nc_a = $c_a\nc_ey_f = $c_ey_f\nc_ev_f = $c_ev_f\nc_epsi_f = $c_epsi_f") close(f) diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py index 6c9bce79..1c3f2c92 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl.py @@ -24,6 +24,7 @@ from system_models import f_KinBkMdl, h_KinBkMdl from tf import transformations from numpy import unwrap, diag +from os.path import expanduser # input variables [default values] d_f = 0 # steering angle [deg] @@ -173,7 +174,7 @@ def state_estimation(): gps_std = rospy.get_param("state_estimation/gps_std") # std of gps measurements t = time.localtime(time.time()) - file = open("/home/odroid/rosbag/info_%i_%02i_%02i_%02i_%02i_%02i.txt"%(t.tm_year,t.tm_mon,t.tm_mday,t.tm_hour,t.tm_min,t.tm_sec),'w') + file = open("%s/rosbag/info_%i_%02i_%02i_%02i_%02i_%02i.txt"%(expanduser('~'),t.tm_year,t.tm_mon,t.tm_mday,t.tm_hour,t.tm_min,t.tm_sec),'w') file.write("EKF parameters\n===============\n") file.write("q = %f\ngps_std = %f\npsi_std = %f\nv_std = %f"%(q_std,gps_std,psi_std,v_std)) file.close() From 69705dd7bac1c6e4c34c9551fcaf5e8c2ad2c71c Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 14 Sep 2016 15:11:12 -0700 Subject: [PATCH 013/183] Merged with branch devel-felix and did little cleanups --- workspace/src/CMakeLists.txt | 2 +- workspace/src/barc/CMakeLists.txt | 4 +- workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/msg/Logging.msg | 2 +- workspace/src/barc/msg/pos_info.msg | 1 + workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 52 +++++++++++-------- workspace/src/barc/src/controller_MPC_traj.jl | 34 ++++++------ .../src/barc/src/state_estimation_KinBkMdl.py | 13 +++-- 9 files changed, 59 insertions(+), 53 deletions(-) diff --git a/workspace/src/CMakeLists.txt b/workspace/src/CMakeLists.txt index 3703df4e..581e61db 120000 --- a/workspace/src/CMakeLists.txt +++ b/workspace/src/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/indigo/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index c05c7b8c..1dc489a5 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -47,12 +47,12 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES + pos_info.msg + Logging.msg Ultrasound.msg Encoder.msg ECU.msg Z_KinBkMdl.msg - Logging.msg - pos_info.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 4423ba82..b92d9498 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -20,7 +20,7 @@ - + diff --git a/workspace/src/barc/msg/Logging.msg b/workspace/src/barc/msg/Logging.msg index 59cac00e..431691e2 100644 --- a/workspace/src/barc/msg/Logging.msg +++ b/workspace/src/barc/msg/Logging.msg @@ -14,4 +14,4 @@ float32[] ey float32[] epsi float32[] v float32[] a -float32[] d_f \ No newline at end of file +float32[] d_f diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index f3ef6225..39f2c782 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -1,3 +1,4 @@ + # This is a message to hold data with position and trajectory information float32 s float32 ey diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 338c3682..129c9e06 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(2.0,2.0,0.1,array([0.0,-1.0]),0) + l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 283d7e75..fa5e0830 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -24,26 +24,26 @@ using data_service.msg using geometry_msgs.msg using sensor_msgs.msg +u_current = zeros(2,1) + function simModel(z,u,dt,l_A,l_B) - bta = atan(L_a/(L_a+L_b)*tan(u[2])) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + bta = atan(l_A/(l_A+l_B)*tan(u[2])) zNext = z zNext[1] = z[1] + dt*(z[4]*cos(z[3]+bta)) # x zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/L_b*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63*z[4]^2 * sign(z[4])) # v + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v return zNext end -# define model parameters -L_a = 0.125 # distance from CoG to front axel -L_b = 0.125 # distance from CoG to rear axel - -u_current = zeros(2,1) - function ECU_callback(msg::ECU) global u_current @@ -57,6 +57,8 @@ function main() pub_gps = Publisher("indoor_gps", Vector3, queue_size=10) pub_imu = Publisher("imu/data", Imu, queue_size=10) + + # read the axle distances from the launch file l_A = get_param("L_a") # distance from CoG to front axel l_B = get_param("L_b") # distance from CoG to rear axel @@ -73,43 +75,47 @@ function main() last_updated = 0 r_tire = 0.036 # radius from tire center to perimeter along magnets [m] - quarterCirc = 0.5 * pi * r_tire + quarterCirc = 0.5 * pi * r_tire # length of a quarter of a tire, distance from one to the next encoder - FL = 0 - FR = 0 - BL = 0 - BR = 0 + FL = 0 #front left wheel encoder counter + FR = 0 #front right wheel encoder counter + BL = 0 #back left wheel encoder counter + BR = 0 #back right wheel encoder counter + println("Publishing sensor information. Simulator running.") while ! is_shutdown() - # Simulate state + # update current state with a new row vector z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' dist_traveled += z_current[i,4]*dt - # Encoder measurements, might be adapted according to the number of encoders on the car + + + # Encoder measurements calculation + dist_traveled += z_current[i,4]*dt #count the total traveled distance since the beginning of the simulation if dist_traveled - last_updated >= quarterCirc last_updated = dist_traveled FL += 1 FR += 1 - BL += 0 - BR += 0 + BL += 1 + BR += 0 #no encoder on back right wheel enc_data = Encoder(FL, FR, BL, BR) - publish(pub_enc, enc_data) + publish(pub_enc, enc_data) #publish a message everytime the encoder counts up end # IMU measurements imu_data = Imu() - imu_data.orientation = geometry_msgs.msg.Quaternion(cos(z_current[i,3]/2), sin(z_current[i,3]/2), 0, 0) + imu_data.orientation = geometry_msgs.msg.Quaternion(cos(z_current[i,3]/2)+randn()*0.01, sin(z_current[i,3]/2)+randn()*0.01, 0, 0) if i%2 == 0 publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end # GPS measurements - x = z_current[i,1]*100 # Indoor gps measures in cm - y = z_current[i,2]*100 - if i % 14 == 0 + x = z_current[i,1]*100 + randn() # Indoor gps measures in cm + y = z_current[i,2]*100 + randn() + if i % 7 == 0 gps_data = Vector3(x,y,0) publish(pub_gps, gps_data) end diff --git a/workspace/src/barc/src/controller_MPC_traj.jl b/workspace/src/barc/src/controller_MPC_traj.jl index 9d9f7c5a..53d751c3 100755 --- a/workspace/src/barc/src/controller_MPC_traj.jl +++ b/workspace/src/barc/src/controller_MPC_traj.jl @@ -32,7 +32,7 @@ dt = 0.1 # time step of system coeffCurvature = [0,0,0,0] # preview horizon -N = 10 +N = 15 # define targets [generic values] v_ref = 0.8 @@ -40,9 +40,9 @@ v_ref = 0.8 # define objective function values c_ey = 50 c_ev = 10 -c_epsi = 5 -c_df = 0.3 -c_a = 0.2 +c_epsi = 50 +c_df = 1 +c_a = 0.36 c_ey_f = 0 c_ev_f = 0 c_epsi_f = 0 @@ -51,17 +51,17 @@ c_epsi_f = 0 # states: position (x,y), yaw angle, and velocity # inputs: acceleration, steering angle println("Creating kinematic bicycle model ....") -mdl = Model(solver = IpoptSolver(print_level=3,max_cpu_time=0.1)) # ,linear_solver="ma97",print_user_options="yes")) +mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1)) @defVar( mdl, s[1:(N+1)] ) @defVar( mdl, ey[1:(N+1)] ) @defVar( mdl, epsi[1:(N+1)] ) @defVar( mdl, 0.0 <= v[1:(N+1)] <= 3.0 ) @defVar( mdl, 2.0 >= a[1:N] >= -1.0 ) -@defVar( mdl, -0.6 <= d_f[1:N] <= 0.6 ) +@defVar( mdl, -0.3 <= d_f[1:N] <= 0.3 ) # define objective function -@setNLObjective(mdl, Min, sum{c_ey*ey[i]^2+c_ev*(v[i]-v_ref)^2+c_epsi*epsi[i]^2+c_df*d_f[i]^2+c_a*a[i]^2,i=1:N} + c_ey_f*ey[N+1]^2 + c_ev_f*(v[N+1]-v_ref)^2 + c_epsi_f*epsi[N+1]^2) +@setNLObjective(mdl, Min, sum{c_ey*ey[i]^2+c_ev*(v[i]-v_ref)^2+c_epsi*epsi[i]^2+c_df*d_f[i]^2+(a[i])^2,i=1:N} + c_ey_f*ey[N+1]^2 + c_ev_f*(v[N+1]-v_ref)^2 + c_epsi_f*epsi[N+1]^2) # define constraints # define system dynamics @@ -92,16 +92,16 @@ println("finished initial solve!") # write info in logfile -currenttime = now() -f = open("$(homedir())/rosbag/MPClog_$currenttime.txt","w") -byteswritten=write(f,"MPC LOG, created $currenttime\n=========================\nv_ref = $v_ref\nN = $N\nc_ey = $c_ey\nc_ev = $c_ev\nc_epsi = $c_epsi\nc_df = $c_df\nc_a = $c_a\nc_ey_f = $c_ey_f\nc_ev_f = $c_ev_f\nc_epsi_f = $c_epsi_f") -close(f) - -if byteswritten>0 - println("Successfully written to Logfile MPClog_$currenttime.txt") -else - println("Problem with Logfile") -end +# currenttime = now() +# f = open("/home/odroid/rosbag/MPClog_$currenttime.txt","w") +# byteswritten=write(f,"MPC LOG, created $currenttime\n=========================\nv_ref = $v_ref\nN = $N\nc_ey = $c_ey\nc_ev = $c_ev\nc_epsi = $c_epsi\nc_df = $c_df\nc_a = $c_a\nc_ey_f = $c_ey_f\nc_ev_f = $c_ev_f\nc_epsi_f = $c_epsi_f") +# close(f) + +# if byteswritten>0 +# println("Successfully written to Logfile MPClog_$currenttime.txt") +# else +# println("Problem with Logfile") +# end function SE_callback(msg::pos_info) diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py index 1c3f2c92..70489b8d 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl.py @@ -74,7 +74,6 @@ def imu_callback(data): ori = data.orientation quaternion = (ori.x, ori.y, ori.z, ori.w) (roll, pitch, yaw) = transformations.euler_from_quaternion(quaternion) - # yaw = -yaw # added for wrong coordinate frame # save initial measurements if not read_yaw0: @@ -174,10 +173,10 @@ def state_estimation(): gps_std = rospy.get_param("state_estimation/gps_std") # std of gps measurements t = time.localtime(time.time()) - file = open("%s/rosbag/info_%i_%02i_%02i_%02i_%02i_%02i.txt"%(expanduser('~'),t.tm_year,t.tm_mon,t.tm_mday,t.tm_hour,t.tm_min,t.tm_sec),'w') - file.write("EKF parameters\n===============\n") - file.write("q = %f\ngps_std = %f\npsi_std = %f\nv_std = %f"%(q_std,gps_std,psi_std,v_std)) - file.close() + # file = open("/home/felix/rosbag/info_%i_%02i_%02i_%02i_%02i_%02i.txt"%(t.tm_year,t.tm_mon,t.tm_mday,t.tm_hour,t.tm_min,t.tm_sec),'w') + # file.write("EKF parameters\n===============\n") + # file.write("q = %f\ngps_std = %f\npsi_std = %f\nv_std = %f"%(q_std,gps_std,psi_std,v_std)) + # file.close() # set node rate loop_rate = 50 dt = 1.0 / loop_rate @@ -200,8 +199,6 @@ def state_estimation(): else: rospy.logerr("No estimation mode selected.") - x_meas = 0 - y_meas = 0 # start loop while not rospy.is_shutdown(): @@ -213,6 +210,7 @@ def state_estimation(): state_pub.publish( Z_KinBkMdl(x_e, y_e, psi_e, v_e) ) # collect measurements, inputs, system properties + if est_mode==1: y = array([x_meas, y_meas, psi_meas, v_meas]) elif est_mode==2: @@ -226,6 +224,7 @@ def state_estimation(): # apply EKF and get each state estimate (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) + # wait rate.sleep() From 99c8fd9f86bac181cb1a86998614bbc441af0c94 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 14 Sep 2016 18:41:51 -0700 Subject: [PATCH 014/183] Added addition logging to simulator (logs measurements, estimation and real values into file), added noise to simulated model, added function to evaluate recorded data (in root dir) --- eval_sim/eval_sim.jl | 28 +++++++ workspace/src/barc/launch/barc_sim.launch | 4 +- workspace/src/barc/src/barc_simulator.jl | 75 ++++++++++++++++--- workspace/src/barc/src/controller_MPC_traj.jl | 54 ++++++------- workspace/src/barc/src/system_models.py | 11 ++- 5 files changed, 131 insertions(+), 41 deletions(-) create mode 100644 eval_sim/eval_sim.jl diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl new file mode 100644 index 00000000..9ecb4d1f --- /dev/null +++ b/eval_sim/eval_sim.jl @@ -0,0 +1,28 @@ +using JLD +using PyPlot + +type Measurements{T} + i::Int64 # measurement counter + t::Array{T} # time data + z::Array{T} # measurement values +end + + +log_path = "$(homedir())/simulations/output.jld" + +function eval_sim() + d = load(log_path) + + est = d["estimate"] + imu_meas = d["imu_meas"] + gps_meas = d["gps_meas"] + z = d["z"] + + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") + grid(1) + legend(["real state","GPS meas","estimate"]) + figure() + plot(z.t,z.z[:,3],imu_meas.t,imu_meas.z,est.t,est.z[:,3]) + grid(1) + legend(["Real psi","psi meas","estimate"]) +end \ No newline at end of file diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index b92d9498..877ab507 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -4,7 +4,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index fa5e0830..72cb2e0d 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -23,8 +23,30 @@ using barc.msg using data_service.msg using geometry_msgs.msg using sensor_msgs.msg +using JLD u_current = zeros(2,1) +t0 = 0 +t = 0 + + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{T} # time data + z::Array{T} # measurement values +end +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + +buffersize = 60000 +gps_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) +imu_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,1)) +est_meas = Measurements{Float32}(0,zeros(Float32,buffersize,1),zeros(Float32,buffersize,4)) +z_real = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,4)) function simModel(z,u,dt,l_A,l_B) @@ -36,20 +58,30 @@ function simModel(z,u,dt,l_A,l_B) zNext = z zNext[1] = z[1] + dt*(z[4]*cos(z[3]+bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + # Add process noise (depending on velocity) + zNext = zNext + 0.01*diagm([0.01*z[4],0.01*z[4],0.001,0.01*z[4]])*randn(4,1) + return zNext end function ECU_callback(msg::ECU) - global u_current u_current = [msg.motor, msg.servo] end +function est_callback(msg::Z_KinBkMdl) + global t0 + t = time() - t0 + est_meas.i += 1 + est_meas.t[est_meas.i] = t + est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] +end + function main() # initiate node, set up publisher / subscriber topics init_node("barc_sim") @@ -63,6 +95,7 @@ function main() l_B = get_param("L_b") # distance from CoG to rear axel s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) + s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) z_current = zeros(60000,4) @@ -82,13 +115,19 @@ function main() BL = 0 #back left wheel encoder counter BR = 0 #back right wheel encoder counter + imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) + + global t0 = time() # Start time of the simulation println("Publishing sensor information. Simulator running.") while ! is_shutdown() + t = time() - t0 + # update current state with a new row vector - z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' - dist_traveled += z_current[i,4]*dt + z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' + z_real.t[i] = t + dist_traveled += z_current[i,4]*dt @@ -106,16 +145,24 @@ function main() # IMU measurements imu_data = Imu() - imu_data.orientation = geometry_msgs.msg.Quaternion(cos(z_current[i,3]/2)+randn()*0.01, sin(z_current[i,3]/2)+randn()*0.01, 0, 0) + imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds + yaw = z_current[i,3] + randn()*0.05 + imu_drift + imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) if i%2 == 0 + imu_meas.i += 1 + imu_meas.t[imu_meas.i] = t + imu_meas.z[imu_meas.i] = yaw publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end # GPS measurements - x = z_current[i,1]*100 + randn() # Indoor gps measures in cm - y = z_current[i,2]*100 + randn() + x = z_current[i,1]*100 + randn()*2 # Indoor gps measures in cm + y = z_current[i,2]*100 + randn()*2 if i % 7 == 0 + gps_meas.i += 1 + gps_meas.t[gps_meas.i] = t + gps_meas.z[gps_meas.i,:] = [x y] gps_data = Vector3(x,y,0) publish(pub_gps, gps_data) end @@ -124,10 +171,20 @@ function main() rossleep(loop_rate) end + # Clean up buffers + + clean_up(gps_meas) + clean_up(est_meas) + clean_up(imu_meas) + z_real.z[1:i-1,:] = z_current[1:i-1,:] + z_real.i = i + clean_up(z_real) + # Save simulation data to file - log_path = "$(homedir())/simulations/output.txt" + log_path = "$(homedir())/simulations/output.jld" + save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"imu_meas",imu_meas) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") - writedlm(log_path,z_current[1:i-1,:]) + #writedlm(log_path,z_current[1:i-1,:]) end if ! isinteractive() diff --git a/workspace/src/barc/src/controller_MPC_traj.jl b/workspace/src/barc/src/controller_MPC_traj.jl index 53d751c3..b8a32a26 100755 --- a/workspace/src/barc/src/controller_MPC_traj.jl +++ b/workspace/src/barc/src/controller_MPC_traj.jl @@ -53,35 +53,35 @@ c_epsi_f = 0 println("Creating kinematic bicycle model ....") mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1)) -@defVar( mdl, s[1:(N+1)] ) -@defVar( mdl, ey[1:(N+1)] ) -@defVar( mdl, epsi[1:(N+1)] ) -@defVar( mdl, 0.0 <= v[1:(N+1)] <= 3.0 ) -@defVar( mdl, 2.0 >= a[1:N] >= -1.0 ) -@defVar( mdl, -0.3 <= d_f[1:N] <= 0.3 ) +@variable( mdl, s[1:(N+1)] ) +@variable( mdl, ey[1:(N+1)] ) +@variable( mdl, epsi[1:(N+1)] ) +@variable( mdl, 0.0 <= v[1:(N+1)] <= 3.0 ) +@variable( mdl, 2.0 >= a[1:N] >= -1.0 ) +@variable( mdl, -0.3 <= d_f[1:N] <= 0.3 ) # define objective function -@setNLObjective(mdl, Min, sum{c_ey*ey[i]^2+c_ev*(v[i]-v_ref)^2+c_epsi*epsi[i]^2+c_df*d_f[i]^2+(a[i])^2,i=1:N} + c_ey_f*ey[N+1]^2 + c_ev_f*(v[N+1]-v_ref)^2 + c_epsi_f*epsi[N+1]^2) +@NLobjective(mdl, Min, sum{c_ey*ey[i]^2+c_ev*(v[i]-v_ref)^2+c_epsi*epsi[i]^2+c_df*d_f[i]^2+(a[i])^2,i=1:N} + c_ey_f*ey[N+1]^2 + c_ev_f*(v[N+1]-v_ref)^2 + c_epsi_f*epsi[N+1]^2) # define constraints # define system dynamics # Reference: R.Rajamani, Vehicle Dynamics and Control, set. Mechanical Engineering Series, # Spring, 2011, page 26 -@defNLParam(mdl, s0 == 0); @addNLConstraint(mdl, s[1] == s0); -@defNLParam(mdl, ey0 == 0); @addNLConstraint(mdl, ey[1] == ey0); -@defNLParam(mdl, epsi0 == 0); @addNLConstraint(mdl, epsi[1] == epsi0 ); -@defNLParam(mdl, v0 == 0); @addNLConstraint(mdl, v[1] == v0); -@defNLParam(mdl, coeff[i=1:length(coeffCurvature)]==coeffCurvature[i]); -@defNLExpr(mdl, c[i = 1:N], coeff[1]*s[i]^3+coeff[2]*s[i]^2+coeff[3]*s[i]+coeff[4]) -@defNLExpr(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( d_f[i]) ) ) +@NLparameter(mdl, s0 == 0); @NLconstraint(mdl, s[1] == s0); +@NLparameter(mdl, ey0 == 0); @NLconstraint(mdl, ey[1] == ey0); +@NLparameter(mdl, epsi0 == 0); @NLconstraint(mdl, epsi[1] == epsi0 ); +@NLparameter(mdl, v0 == 0); @NLconstraint(mdl, v[1] == v0); +@NLparameter(mdl, coeff[i=1:length(coeffCurvature)]==coeffCurvature[i]); +@NLexpression(mdl, c[i = 1:N], coeff[1]*s[i]^3+coeff[2]*s[i]^2+coeff[3]*s[i]+coeff[4]) +@NLexpression(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( d_f[i]) ) ) -@defNLExpr(mdl, dsdt[i = 1:N], v[i]*cos(epsi[i]+bta[i])/(1-ey[i]*c[i])) +@NLexpression(mdl, dsdt[i = 1:N], v[i]*cos(epsi[i]+bta[i])/(1-ey[i]*c[i])) for i in 1:N - @addNLConstraint(mdl, s[i+1] == s[i] + dt*dsdt[i] ) - @addNLConstraint(mdl, ey[i+1] == ey[i] + dt*v[i]*sin(epsi[i]+bta[i]) ) - @addNLConstraint(mdl, epsi[i+1] == epsi[i] + dt*(v[i]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) - @addNLConstraint(mdl, v[i+1] == v[i] + dt*(a[i] - 0.63 *abs(v[i])*v[i]) ) + @NLconstraint(mdl, s[i+1] == s[i] + dt*dsdt[i] ) + @NLconstraint(mdl, ey[i+1] == ey[i] + dt*v[i]*sin(epsi[i]+bta[i]) ) + @NLconstraint(mdl, epsi[i+1] == epsi[i] + dt*(v[i]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) + @NLconstraint(mdl, v[i+1] == v[i] + dt*(a[i] - 0.63 *abs(v[i])*v[i]) ) end @@ -106,11 +106,11 @@ println("finished initial solve!") function SE_callback(msg::pos_info) # update mpc initial condition - setValue(s0, msg.s) - setValue(ey0, msg.ey) - setValue(epsi0, msg.epsi) - setValue(v0, msg.v) - setValue(coeff, msg.coeffCurvature) + setvalue(s0, msg.s) + setvalue(ey0, msg.ey) + setvalue(epsi0, msg.epsi) + setvalue(v0, msg.v) + setvalue(coeff, msg.coeffCurvature) end function main() @@ -129,8 +129,8 @@ function main() solvetime = toc() if status == Symbol("Optimal") # get optimal solutions - a_opt = getValue(a[1]) - d_f_opt = getValue(d_f[1]) + a_opt = getvalue(a[1]) + d_f_opt = getvalue(d_f[1]) cmd = ECU(a_opt, d_f_opt) # publish commands if cmdcount>10 # ignore first 10 commands since MPC often stagnates during the first seconds (why?) @@ -145,7 +145,7 @@ function main() end end println("Solve Status: ", string(status), "\nSolve Time: ", solvetime,"\n\n") - loginfo = Logging(getObjectiveValue(mdl),string(status),solvetime,getValue(s),getValue(ey),getValue(epsi),getValue(v),getValue(a),getValue(d_f)) + loginfo = Logging(getobjectivevalue(mdl),string(status),solvetime,getvalue(s),getvalue(ey),getvalue(epsi),getvalue(v),getvalue(a),getvalue(d_f)) publish(pub2, loginfo) cmdcount = cmdcount + 1 rossleep(loop_rate) diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index e3e05d12..3c258677 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -226,9 +226,14 @@ def h_KinBkMdl(x): """ measurement model """ + # For GPS, IMU and encoders: + # C = array([[1, 0, 0, 0], + # [0, 1, 0, 0], + # [0, 0, 1, 0], + # [0, 0, 0, 1]]) + + # For GPS only: C = array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]]) + [0, 1, 0, 0]]) return dot(C, x) From b566d4b2552d1c2cf9a4ff4b141c7d24bdf54734 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 14 Sep 2016 20:31:29 -0700 Subject: [PATCH 015/183] Changed message format to float64, added s_start to message pos_info, restructured LMPC_node so that all variables can be used (some have to be global), changed a few types --- workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/msg/ECU.msg | 4 +- workspace/src/barc/msg/pos_info.msg | 17 +++-- workspace/src/barc/src/LMPC_node.jl | 76 ++++++++++++++----- .../src/barc/src/Localization_helpers.py | 3 +- workspace/src/barc/src/Localization_node.py | 4 +- workspace/src/barc/src/barc_simulator.jl | 4 +- workspace/src/barc/src/helper/classes.jl | 7 +- .../barc/src/helper/coeffConstraintCost.jl | 1 + workspace/src/barc/src/helper/createModel.jl | 19 +---- .../src/barc/src/helper/solveMpcProblem.jl | 3 + 11 files changed, 86 insertions(+), 54 deletions(-) mode change 100644 => 100755 workspace/src/barc/src/LMPC_node.jl diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 877ab507..03ebb85e 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -32,7 +32,7 @@ - + diff --git a/workspace/src/barc/msg/ECU.msg b/workspace/src/barc/msg/ECU.msg index 2bffecbf..823acb59 100644 --- a/workspace/src/barc/msg/ECU.msg +++ b/workspace/src/barc/msg/ECU.msg @@ -6,5 +6,5 @@ # # For modeling and state estimation, motors units are [N], and servo units are [rad] # For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle -float32 motor -float32 servo +float64 motor +float64 servo diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 39f2c782..03a94251 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -1,10 +1,11 @@ # This is a message to hold data with position and trajectory information -float32 s -float32 ey -float32 epsi -float32 v -float32[] coeffX -float32[] coeffY -float32[] coeffTheta -float32[] coeffCurvature +float64 s +float64 ey +float64 epsi +float64 v +float64 s_start +float64[] coeffX +float64[] coeffY +float64[] coeffTheta +float64[] coeffCurvature diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl old mode 100644 new mode 100755 index f41707b1..50bb9ca6 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -15,39 +15,37 @@ include("helper/classes.jl") include("helper/coeffConstraintCost.jl") include("helper/solveMpcProblem.jl") include("helper/computeCostLap.jl") -include("simModel.jl") # Load Variables and create Model: println("Loading and defining variables...") -include("createModel.jl") +include("helper/createModel.jl") # Initialize model by solving it once println("Initial solve...") solve(mdl) println("Finished initial solve.") -QderivZ = 0.0*[1 1 1 1] # cost matrix for derivative cost of states -QderivU = 0.1*[1 1] # cost matrix for derivative cost of inputs -mpcParams.R = 0.0*[1 1] # cost matrix for control inputs -mpcParams.Q = [0.0 10.0 1.0 1.0] # put weights on ey, epsi and v -cost = zeros(length(t),6) -posInfo.s_target = 6 +z_est = zeros(1,4) -trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) -z_final = zeros(1,4) -u_final = zeros(1,2) -z_est = zeros(1,4) +posInfo = PosInfo() +posInfo.s_start = 0 +posInfo.s_target = 10.2812 function SE_callback(msg::pos_info) # update current position and track data - # update mpc initial condition + # update mpc initial condition + global trackCoeff, z_est, posInfo z_est = [msg.s msg.ey msg.epsi msg.v] + posInfo.s = msg.s posInfo.s_start = msg.s_start trackCoeff.coeffCurvature = msg.coeffCurvature + #println("Received pos info: $msg") + #println("coeffCurvature = $(trackCoeff.coeffCurvature)") end function main() + println("now starting the node") # initiate node, set up publisher / subscriber topics init_node("mpc_traj") pub = Publisher("ecu", ECU, queue_size=10) @@ -55,25 +53,68 @@ function main() s1 = Subscriber("pos_info", pos_info, SE_callback, queue_size=10) loop_rate = Rate(10) - lapStatus.currentLap = 1 # initialize lap number - lapStatus.currentIt = 1 # current iteration in lap + # Define and initialize variables + global mpcParams, trackCoeff, modelParams, z_est, posInfo + oldTraj = OldTrajectory() + mpcCoeff = MpcCoeff() + lapStatus = LapStatus(1,1) + mpcSol = MpcSol() + + mpcParams.QderivZ = 0.0*[1 1 1 1] # cost matrix for derivative cost of states + mpcParams.QderivU = 0.1*[1 1] # cost matrix for derivative cost of inputs + mpcParams.R = 0.0*[1 1] # cost matrix for control inputs + mpcParams.Q = [0.0 10.0 1.0 1.0] # put weights on ey, epsi and v + + buffersize = 700 + oldTraj.oldTraj = zeros(4,buffersize,2) + oldTraj.oldInput = zeros(2,buffersize,2) + + mpcCoeff.coeffCost = 0 + mpcCoeff.coeffConst = 0 + mpcCoeff.order = 5 + mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon + + lapStatus.currentLap = 1 # initialize lap number + lapStatus.currentIt = 1 # current iteration in lap + + + + # Lap parameters switchLap = false # initialize lap lap trigger s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] + # buffer in current lap zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information + println("Starting loop") while ! is_shutdown() + + # global trackCoeff, mpcParams # do they need to be within the while loop? + if posInfo.s_start + posInfo.s >= posInfo.s_target # if we are behind the finish line + posInfo.s_start = 0 # then set s_start to 0 + posInfo.s = posInfo.s_start + posInfo.s - posInfo.s_target # and calculate new current s + end + lapStatus.currentIt += 1 # count iteration + println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") + i = lapStatus.currentIt # just to make notation shorter zCurr[i,:] = z_est # update state information + println("State Nr. $i = $z_est") + println("Coeff Curvature = $(trackCoeff.coeffCurvature)") + # Find coefficients for cost and constraints mpcCoeff = coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + println("Found coefficients: mpcCoeff = $mpcCoeff") # Solve the MPC problem mpcSol = solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i-1,:]',uCurr[i-1,:]') # Write in current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] + println("posInfo = $posInfo") + #println("trackCoeff = $trackCoeff") + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:])") # ... and publish data cmd = ECU(mpcSol.a_x, mpcSol.d_f) publish(pub, cmd) @@ -81,7 +122,7 @@ function main() # ================================== # Lap trigger ====================== # ================================== - if posInfo.s_start < s_lapTrigger && switchLap # if we are switching to the next lap... + if posInfo.s_start + posInfo.s <= s_lapTrigger && switchLap # if we are switching to the next lap... # ... then select and save data zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) @@ -109,7 +150,8 @@ function main() switchLap = false zCurr[1,:] = zCurr[i,:] uCurr[1,:] = uCurr[i,:] - else + println("=========================== NEXT LAP ========================== ") + elseif posInfo.s_start > s_lapTrigger switchLap = true end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index d5bfba0f..fe9c8f43 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -18,7 +18,7 @@ class Localization: ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total OrderXY = 6 # order of x-y-polynomial interpolation - OrderThetaCurv = 3 # order of theta interpolation + OrderThetaCurv = 4 # order of theta interpolation closed = True # open or closed trajectory? coeffX = 0 @@ -83,6 +83,7 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (equi # ds = hstack((ds,((x[n-1]-x[0])**2+(y[n-1]-y[0])**2)**0.5)) s = hstack((0,cumsum(ds))) length = sum(ds) + print "Track length = %fm"%length dsn = ds_in # optimal new step size (might be calculated externally for now) rem = lambda x:mod(length,x) # function to calculate optimal step size (for evenly distributed steps) sol = scipy.optimize.fmin(func=rem,x0=dsn) diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 129c9e06..9485c9e5 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) + l.create_racetrack(2.0,2.0,0.2,array([-0.5,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) @@ -53,7 +53,7 @@ def localization_node(): while not rospy.is_shutdown(): # publish information - state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.coeffX,l.coeffY,l.coeffTheta,l.coeffCurvature) ) + state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.coeffX,l.coeffY,l.coeffTheta,l.coeffCurvature) ) # wait rate.sleep() diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 72cb2e0d..e3f2de6e 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -157,8 +157,8 @@ function main() end # GPS measurements - x = z_current[i,1]*100 + randn()*2 # Indoor gps measures in cm - y = z_current[i,2]*100 + randn()*2 + x = round(z_current[i,1]*100 + randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index ceb1e542..77b3ce6a 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -35,15 +35,16 @@ type MpcParams # parameters for MPC solver Q R vPathFollowing::Float64 - MpcParams(N=0,nz=0,OrderCostCons=0,Q=0,R=0,vPathFollowing=1.0) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) + QderivZ + QderivU + MpcParams(N=0,nz=0,OrderCostCons=0,Q=0,R=0,vPathFollowing=1.0,QderivZ=0.0,QderivU=0.0) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) end type PosInfo # current position information s_start::Float64 s::Float64 - s_switch::Float64 s_target::Float64 - PosInfo(s_start=0,s=0,s_switch=0,s_target=0) = new(s_start,s,s_switch,s_target) + PosInfo(s_start=0,s=0,s_target=0) = new(s_start,s,s_target) end type MpcSol # MPC solution output diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index 0e9d3add..ae292224 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -185,6 +185,7 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) else # if it is the first lap + println("First lap, so everything's zero..") coeffCost = zeros(Order+1,1,2) coeffConst = zeros(nz-1,Order+1,1,2) # nz-1 because no coeff for s end diff --git a/workspace/src/barc/src/helper/createModel.jl b/workspace/src/barc/src/helper/createModel.jl index b501b9a1..9c524998 100644 --- a/workspace/src/barc/src/helper/createModel.jl +++ b/workspace/src/barc/src/helper/createModel.jl @@ -1,35 +1,18 @@ # Define Variables -oldTraj = OldTrajectory() -lapStatus = LapStatus(1,1) -mpcCoeff = MpcCoeff() -posInfo = PosInfo() -mpcParams = MpcParams() -stateIn = zeros(7,1) # stateIn: xdot, ydot, psidot, epsi, y, s, a_f -inputIn = zeros(2,1) -mpcSol = MpcSol() trackCoeff = TrackCoeff() # info about track (at current position, approximated) modelParams = ModelParams() +mpcParams = MpcParams() # =============================== # Initialize Parameters # =============================== -buffersize = 700 -oldTraj.oldTraj = zeros(4,buffersize,2) -oldTraj.oldInput = zeros(2,buffersize,2) -posInfo.s_start = 0 -posInfo.s_target = 2 mpcParams.N = 5 mpcParams.nz = 4 mpcParams.Q = [0.0 1.0 1.0 1.0] # put weights on ey, epsi and v mpcParams.vPathFollowing = 0.2 -mpcCoeff.coeffCost = 0 -mpcCoeff.coeffConst = 0 -mpcCoeff.order = 5 -mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon - trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation trackCoeff.width = 0.4 # width of the track (0.5m) diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index b40225c0..15e5841b 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -21,6 +21,9 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa s_target = posInfo.s_target ey_max = trackCoeff.width/2 + QderivZ = mpcParams.QderivZ + QderivU = mpcParams.QderivU + v_ref = mpcParams.vPathFollowing if lapStatus.currentLap > 1 #v_ref = 10 From 1bb4eaeeb26aca3bf404a6d02f7753fe7baa5d3a Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 15 Sep 2016 13:37:53 -0700 Subject: [PATCH 016/183] Modified racetrack creation: Start/Finish is at middle of straight line. Fixed a couple of LMPC things, it's still pretty slow when it comes to calculating the mpcCoefficients the first time in the second lap. --- eval_sim/eval_sim.jl | 8 + workspace/src/barc/src/LMPC_node.jl | 174 ++++++++++-------- .../src/barc/src/Localization_helpers.py | 6 +- workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 6 +- workspace/src/barc/src/helper/README.txt | 39 ++++ workspace/src/barc/src/helper/classes.jl | 8 +- .../barc/src/helper/coeffConstraintCost.jl | 29 ++- workspace/src/barc/src/helper/createModel.jl | 6 +- .../src/barc/src/helper/solveMpcProblem.jl | 50 +++-- 10 files changed, 213 insertions(+), 115 deletions(-) create mode 100644 workspace/src/barc/src/helper/README.txt diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 9ecb4d1f..778c2ea4 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -9,6 +9,7 @@ end log_path = "$(homedir())/simulations/output.jld" +log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" function eval_sim() d = load(log_path) @@ -25,4 +26,11 @@ function eval_sim() plot(z.t,z.z[:,3],imu_meas.t,imu_meas.z,est.t,est.z[:,3]) grid(1) legend(["Real psi","psi meas","estimate"]) +end + +function eval_LMPC() + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + plot(oldTraj[:,:,1,1]) + grid(1) end \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 50bb9ca6..d804465a 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -10,6 +10,7 @@ using data_service.msg using geometry_msgs.msg using JuMP using Ipopt +#using JLD include("helper/classes.jl") include("helper/coeffConstraintCost.jl") @@ -31,15 +32,16 @@ z_est = zeros(1,4) posInfo = PosInfo() posInfo.s_start = 0 -posInfo.s_target = 10.2812 +posInfo.s_target = 10.281192 +coeffCurvature_update = 0 # use extra update variables so that they're not suddenly changed within functions +s_start_update = 0 function SE_callback(msg::pos_info) # update current position and track data # update mpc initial condition - global trackCoeff, z_est, posInfo + global z_est, posInfo, coeffCurvature_update, s_start_update z_est = [msg.s msg.ey msg.epsi msg.v] - posInfo.s = msg.s - posInfo.s_start = msg.s_start - trackCoeff.coeffCurvature = msg.coeffCurvature + s_start_update = msg.s_start + coeffCurvature_update = msg.coeffCurvature #println("Received pos info: $msg") #println("coeffCurvature = $(trackCoeff.coeffCurvature)") end @@ -53,8 +55,13 @@ function main() s1 = Subscriber("pos_info", pos_info, SE_callback, queue_size=10) loop_rate = Rate(10) + buffersize = 700 + + # Create data to be saved + # save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps + # Define and initialize variables - global mpcParams, trackCoeff, modelParams, z_est, posInfo + global mpcParams, trackCoeff, modelParams, z_est, posInfo, coeffCurvature_update, s_start_update oldTraj = OldTrajectory() mpcCoeff = MpcCoeff() lapStatus = LapStatus(1,1) @@ -65,7 +72,6 @@ function main() mpcParams.R = 0.0*[1 1] # cost matrix for control inputs mpcParams.Q = [0.0 10.0 1.0 1.0] # put weights on ey, epsi and v - buffersize = 700 oldTraj.oldTraj = zeros(4,buffersize,2) oldTraj.oldInput = zeros(2,buffersize,2) @@ -75,9 +81,7 @@ function main() mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon lapStatus.currentLap = 1 # initialize lap number - lapStatus.currentIt = 1 # current iteration in lap - - + lapStatus.currentIt = 0 # current iteration in lap # Lap parameters switchLap = false # initialize lap lap trigger @@ -89,74 +93,100 @@ function main() println("Starting loop") while ! is_shutdown() - - # global trackCoeff, mpcParams # do they need to be within the while loop? - if posInfo.s_start + posInfo.s >= posInfo.s_target # if we are behind the finish line - posInfo.s_start = 0 # then set s_start to 0 - posInfo.s = posInfo.s_start + posInfo.s - posInfo.s_target # and calculate new current s - end - - lapStatus.currentIt += 1 # count iteration - println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") - - i = lapStatus.currentIt # just to make notation shorter - zCurr[i,:] = z_est # update state information - - println("State Nr. $i = $z_est") - println("Coeff Curvature = $(trackCoeff.coeffCurvature)") - - # Find coefficients for cost and constraints - mpcCoeff = coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) - println("Found coefficients: mpcCoeff = $mpcCoeff") - # Solve the MPC problem - mpcSol = solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i-1,:]',uCurr[i-1,:]') - # Write in current input information - uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] - println("posInfo = $posInfo") - #println("trackCoeff = $trackCoeff") - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:])") - # ... and publish data - cmd = ECU(mpcSol.a_x, mpcSol.d_f) - publish(pub, cmd) - - # ================================== - # Lap trigger ====================== - # ================================== - if posInfo.s_start + posInfo.s <= s_lapTrigger && switchLap # if we are switching to the next lap... - # ... then select and save data - zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) - uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) - costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line - # Save all data in oldTrajectory: - if lapStatus.currentLap == 1 # if it's the first lap - oldTraj.oldTraj[:,:,1] = zCurr_export' # ... just save everything - oldTraj.oldInput[:,:,1] = uCurr_export' - oldTraj.oldTraj[:,:,2] = zCurr_export' - oldTraj.oldInput[:,:,2] = uCurr_export' - oldTraj.oldCost = [costLap,costLap] - else # idea: always copy the new trajectory in the first array! - if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second - oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second - oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input - oldTraj.oldCost[2] = oldTraj.oldCost[1] + if z_est[1] > 0 # check if data has been received (s > 0) + + # ============================= Initialize iteration parameters ============================= + lapStatus.currentIt += 1 # count iteration + + i = lapStatus.currentIt # current iteration number, just to make notation shorter + zCurr[i,:] = z_est # update state information + posInfo.s = z_est[1] # update position info + posInfo.s_start = s_start_update + trackCoeff.coeffCurvature = coeffCurvature_update + + + # ======================================= Lap trigger ======================================= + if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... + # ... then select and save data + tic() + zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) + uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) + costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line + # Save all data in oldTrajectory: + if lapStatus.currentLap == 1 # if it's the first lap + oldTraj.oldTraj[:,:,1] = zCurr_export' # ... just save everything + oldTraj.oldInput[:,:,1] = uCurr_export' + oldTraj.oldTraj[:,:,2] = zCurr_export' + oldTraj.oldInput[:,:,2] = uCurr_export' + oldTraj.oldCost = [costLap,costLap] + else # idea: always copy the new trajectory in the first array! + if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second + oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second + oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input + oldTraj.oldCost[2] = oldTraj.oldCost[1] + end + oldTraj.oldTraj[:,:,1] = zCurr_export' # ... and write the new traj in the first + oldTraj.oldInput[:,:,1] = uCurr_export' + oldTraj.oldCost[1] = costLap end - oldTraj.oldTraj[:,:,1] = zCurr_export' # ... and write the new traj in the first - oldTraj.oldInput[:,:,1] = uCurr_export' - oldTraj.oldCost[1] = costLap + #println(size(save_oldTraj)) + #println(size(oldTraj.oldTraj)) + #save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state + uCurr[1,:] = uCurr[i+1,:] # ... and input + i = 1 + lapStatus.currentLap += 1 # start next lap + lapStatus.currentIt = 1 # reset current iteration + switchLap = false + tt = toc() + println("======================================== NEXT LAP ========================================") + println("oldTraj.oldTraj[1,:,1]:") + println(oldTraj.oldTraj[1,:,1]) + elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger + switchLap = true end - lapStatus.currentLap += 1 # start next lap - lapStatus.currentIt = 1 # reset current iteration - switchLap = false - zCurr[1,:] = zCurr[i,:] - uCurr[1,:] = uCurr[i,:] - println("=========================== NEXT LAP ========================== ") - elseif posInfo.s_start > s_lapTrigger - switchLap = true - end + # ======================================= Calculate input ======================================= + println("======================================== NEW ITERATION # $i ========================================") + println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") + println("State Nr. $i = $z_est") + println("Coeff Curvature = $(trackCoeff.coeffCurvature)") + println("posInfo = $posInfo") + println("s = $(posInfo.s)") + println("s_start = $(posInfo.s_start)") + println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") + + # Find coefficients for cost and constraints + tic() + mpcCoeff = coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + tt = toc() + println("Finished coefficients, t = $tt s") + #println("Found coefficients: mpcCoeff = $mpcCoeff") + # Solve the MPC problem + tic() + mpcSol = solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + tt = toc() + # Write in current input information + uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] + #println("trackCoeff = $trackCoeff") + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") + # ... and publish data + cmd = ECU(mpcSol.a_x, mpcSol.d_f) + publish(pub, cmd) + + zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) + println("\n") + + else + println("No estimation data received!") + end rossleep(loop_rate) end + # Save simulation data to file + # log_path = "$(homedir())/simulations/LMPC_output.jld" + # save(log_path,"oldTraj",save_oldTraj) + # println("Exiting LMPC node. Saved data.") end if ! isinteractive() diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index fe9c8f43..cf9e4f3f 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -44,17 +44,19 @@ def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to make sur #self.ds = rad*2*pi/n self.ds = 2*rad*tan(2*pi/n/2) def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points - x = arange(-L/2.0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines + x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines s = size(x) da = 2*pi/360*5 x = hstack((x,L/2.0+b/2*cos(arange(pi/2,-pi/2,-da)))) - x = hstack((x,arange(L/2.0,-L/2.0,-ds))) + x = hstack((x,linspace(L/2.0,-L/2.0,5))) x = hstack((x,-L/2.0+b/2*cos(arange(pi/2,1.5*pi+da,da)))) + x = hstack((x,linspace(-L/2.0,0,5))) y = ones(s)*b/2.0 y = hstack((y,b/2*sin(arange(pi/2,-pi/2,-da)))) y = hstack((y,-ones(s)*b/2)) y = hstack((y,-b/2*sin(arange(pi/2,1.5*pi+da,da)))) + y = hstack((y,ones(s)*b/2.0)) s_tot = size(x) diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 9485c9e5..34fb7c94 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(2.0,2.0,0.2,array([-0.5,-1.0]),0) + l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index e3f2de6e..41304a13 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -146,7 +146,7 @@ function main() # IMU measurements imu_data = Imu() imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds - yaw = z_current[i,3] + randn()*0.05 + imu_drift + yaw = z_current[i,3] + 0*(randn()*0.05 + imu_drift) imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) if i%2 == 0 imu_meas.i += 1 @@ -157,8 +157,8 @@ function main() end # GPS measurements - x = round(z_current[i,1]*100 + randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + randn()*2) + x = round(z_current[i,1]*100 + 0*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 0*randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/helper/README.txt b/workspace/src/barc/src/helper/README.txt new file mode 100644 index 00000000..c1a2e0c7 --- /dev/null +++ b/workspace/src/barc/src/helper/README.txt @@ -0,0 +1,39 @@ +This file helps to explain the structure of the LMPC control. + + +1. The estimator and Localization node +====================================== +The estimator estimates the current state (x, y, psi, v) by using a Kalman filter on the sensor data. +The localization node maps this estimated data on the given track and calculates following values: +s_start = distance along the track, starting from the start/finish line to where a polynomial approximation of the track's curvature starts +s = distance along the track, starting at s_start, to the current position of the car +eY = distance of the car to the track (perpendicular to track) +ePsi = difference of the car's heading and the current reference heading (tangent to the track) +v = current velocity +The track's shape has to be defined in Localization_node.py. + +2. The LMPC node +================ +The LMPC node receives the position info (s_start, s, eY, ePsi, v) and first calculates the mpc coefficients (they are part of the "learning" part and account for previous laps). The coefficients approximate polynomials that are used in the MPC control, in the interval closest_s to closest_s + 2*N. +These coefficients are then sent to the solveMpcProblem.jl function which calculates the new optimal input. + + +Further info +============ +Confusion with different s's: +s_target = length of the track + +Localization node: +------------------ +It returns s_start in the interval of 0 < s_start < s_target and s in the interval 0 < s < nPoints*ds (with nPoints = number of nodes along the track which are used to calculate the approximate polynomial and ds = distance between nodes). +s, s_start and the polynomial approximation coefficients are received by the LMPC_node and written in the state *and* posInfo. +Important: The polynomial coefficients are only valid in the interval 0 < s < nPoints*ds, starting from s_start !!! + +coeffConstraintCost: +-------------------- +This function calculates coefficients which approximate the previous trajectory around the current position s_total = s_start + s. The approximated polynomials are valid in the range s_total < s_poly < s_total + 2*N (with N = prediction horizon of the MPC). + +solveMpcProblem: +---------------- +This is the MPC solver, it calculates the optimal input controls by predicting the state evolution, starting at the current state. It uses the states [s, ey, epsi, v] and the coefficients from the Localization node (track coefficients) for calculating the evolution. Therefore, it is important that for the state evolution the relative value s is used! +However, there's a second part to the MPC, which calculates terminal constraints and cost (heart of the learning part). \ No newline at end of file diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index 77b3ce6a..a2c396b7 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -22,10 +22,10 @@ type MpcCoeff # coefficients for trajectory approximation end type OldTrajectory # information about previous trajectories - oldTraj - oldInput - oldCost - OldTrajectory(oldTraj=0,oldInput=0,oldCost=0) = new(oldTraj,oldInput,oldCost) + oldTraj::Array{Float64} + oldInput::Array{Float64} + oldCost::Array{Int64} + OldTrajectory(oldTraj=zeros(100,4,2),oldInput=zeros(100,2,2),oldCost=[0,0]) = new(oldTraj,oldInput,oldCost) end type MpcParams # parameters for MPC solver diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index ae292224..ed27f5e1 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -10,7 +10,7 @@ # structure of oldTrajectory: 1st dimension = state number, 2nd dimension = step number (time equiv.), 3rd dimennsion = lap number -function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) +function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams) # this computes the coefficients for the cost and constraints # Outputs: @@ -18,6 +18,7 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) # coeffCost # Read Inputs + tic() oldTrajectory = oldTraj.oldTraj # [:,:,1] = 1st, [:,:,2] = 2nd oldInput = oldTraj.oldInput @@ -48,11 +49,14 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) N_points = size(oldTrajectory,2) # second dimension = length if lapNumber > 1 + tt=toc() + println("Loading parameters: $tt") + tic() deltaOld = oldInput[1,:,:] aOld = oldInput[2,:,:] # Compute the total s (current position along track) - s_total = s_start + s + s_total = (s_start + s) % s_target # Compute the index DistS = ( s_total - oldS ).^2 @@ -74,12 +78,24 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) beY_Vector = cat(3, oldeY[vec_range[1]], oldeY[vec_range[2]]) bePsi_Vector = cat(3, oldePsi[vec_range[1]], oldePsi[vec_range[2]]) bV_Vector = cat(3, oldV[vec_range[1]], oldV[vec_range[2]]) + + tt = toc() + println("2nd: $tt") + tic() + println("************************************** COEFFICIENTS **************************************") + println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") + println("s_total = $s_total") + # println("bS_Vector[1] = $(bS_Vector[:,:,1]')") # These matrices (above) contain two vectors each (for both old trajectories), stored in the 3rd dimension # The states are parametrized with resprect to the curvilinear abscissa, # so we select the point used for the interpolation. Need to subtract an # offset to be coherent with the MPC formulation s_forinterpy = bS_Vector - s_start + if s_total - s_start < 0 + s_forinterpy += s_target + end + println("s_forinterpy[:,1,1]' = $(s_forinterpy[:,1,1]')") # Create the Matrices for the interpolation MatrixInterp = zeros(pLength+1,Order+1,2) @@ -91,17 +107,22 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) CoefficientsFor_ey = zeros(Order+1,1,2) CoefficientsFor_ePsi = zeros(Order+1,1,2) CoefficientsFor_V = zeros(Order+1,1,2) + println("Calculating coefficients...") for i=1:2 CoefficientsFor_ey[:,:,i] = MatrixInterp[:,:,i]\beY_Vector[:,:,i] CoefficientsFor_ePsi[:,:,i] = MatrixInterp[:,:,i]\bePsi_Vector[:,:,i] CoefficientsFor_V[:,:,i] = MatrixInterp[:,:,i]\bV_Vector[:,:,i] end + println("Done.") # Stack the coefficients coeffConst = zeros(3,Order+1,1,2) coeffConst[1,:,:,:] = CoefficientsFor_ey coeffConst[2,:,:,:] = CoefficientsFor_ePsi coeffConst[3,:,:,:] = CoefficientsFor_V + tt = toc() + println("3rd: $tt") + tic() # structure of coeffConst: # 1st dimension specifies state # 2nd dimension specifies steps @@ -168,7 +189,10 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) # grid() # readline() # end + end + tt = toc() + println("4th: $tt") # if maximum(coeffConst) > 1e4 # warn("Large coefficients in constraints, might cause numerical problems.") @@ -185,7 +209,6 @@ function coeffConstraintCost(oldTraj, lapStatus, mpcCoeff, posInfo, mpcParams) else # if it is the first lap - println("First lap, so everything's zero..") coeffCost = zeros(Order+1,1,2) coeffConst = zeros(nz-1,Order+1,1,2) # nz-1 because no coeff for s end diff --git a/workspace/src/barc/src/helper/createModel.jl b/workspace/src/barc/src/helper/createModel.jl index 9c524998..c80999e6 100644 --- a/workspace/src/barc/src/helper/createModel.jl +++ b/workspace/src/barc/src/helper/createModel.jl @@ -17,8 +17,8 @@ trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coeffic trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation trackCoeff.width = 0.4 # width of the track (0.5m) -modelParams.u_lb = [-2.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering -modelParams.u_ub = [2.0 pi/6]' * ones(1,mpcParams.N) # upper bounds +modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering +modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) @@ -38,7 +38,7 @@ L_b = modelParams.l_B N = mpcParams.N c0 = modelParams.c0 -mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.01))#,linear_solver="ma57",print_user_options="yes")) +mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1))#,linear_solver="ma57",print_user_options="yes")) #@variable( mdl, modelParams.z_ub[i,j] >= z_Ol[i=1:4,j=1:(N+1)] >= modelParams.z_lb[i,j]) # z = s, ey, epsi, v @variable( mdl, z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 15e5841b..8ed80478 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -21,16 +21,21 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa s_target = posInfo.s_target ey_max = trackCoeff.width/2 + #zCurr[1] = posInfo.s # current relative s (used in interpolation) + QderivZ = mpcParams.QderivZ QderivU = mpcParams.QderivU v_ref = mpcParams.vPathFollowing - if lapStatus.currentLap > 1 - #v_ref = 10 - end global mdl + println("************************************** MPC SOLVER **************************************") + println("zCurr = $(zCurr')") + println("s_start = $s_start") + println("s_target = $s_target") + println("s_total = $((zCurr[1]+s_start)%s_target)") + # Create function-specific parameters z_Ref = cat(1,s_target*ones(1,N+1),zeros(2,N+1),v_ref*ones(1,N+1)) # Reference trajectory: path following -> stay on line and keep constant velocity u_Ref = zeros(2,N) @@ -53,7 +58,7 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa # Lane cost # --------------------------------- - @NLexpression(mdl, laneCost, 10*sum{(0.5+0.5*tanh(50*(z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(z_Ol[2,i]+ey_max))),i=1:N+1}) + @NLexpression(mdl, laneCost, 0*sum{(0.5+0.5*tanh(50*(z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(z_Ol[2,i]+ey_max))),i=1:N+1}) # Control Input cost # --------------------------------- @@ -73,34 +78,25 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa # Terminal cost # --------------------------------- - if zCurr[1] + s_start > s_target # if finish line crossed - println("Crossed finish line. I should not be here.") - @NLexpression(mdl, costZTerm, 0) # terminal cost is zero - else - if lapStatus.currentLap > 2 # if at least in the 3rd lap - # costZTerm = ParInt*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + - # + (1-ParInt)*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl, costZTerm, ParInt*sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermCost[i,1,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) - #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) - # line above not necessary since the polynomial goes to zero anyways! - elseif lapStatus.currentLap == 2 # if we're in the second second lap - # costZTerm = polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl, costZTerm, sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) - #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) - end + if lapStatus.currentLap > 2 # if at least in the 3rd lap + # costZTerm = ParInt*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + + # + (1-ParInt)*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); + @NLexpression(mdl, costZTerm, ParInt*sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermCost[i,1,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) + # line above not necessary since the polynomial goes to zero anyways! + elseif lapStatus.currentLap == 2 # if we're in the second second lap + # costZTerm = polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); + @NLexpression(mdl, costZTerm, sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) end # State cost # --------------------------------- - if lapStatus.currentLap == 1 || zCurr[1] + s_start > s_target # if we're in the first lap or crossed finish line, just do path following - # currCostZ = 0.5*Q[j]*pow(ZOlGlobal[(i+1)*nz+j]-ZRefGlobal[(i+1)*nz+j],2); - if lapStatus.currentLap > 1 - println("I should not be here.") - end + if lapStatus.currentLap == 1 # if we're in the first lap, just do path following @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[i,j]-z_Ref[i,j])^2,j=1:N+1},i=1:4}) # Follow trajectory - #println(z_Ref) - else + + else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) #@NLexpression(mdl, costZ_h, 0) # zero state cost after crossing the finish line #@NLexpression(mdl, costZ, 1 + (costZ_h-1) * (0.5+0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) @NLexpression(mdl, costZ, 1) From afc1d789368693e4a85d909f03a238379ffa78f5 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 15 Sep 2016 18:26:08 -0700 Subject: [PATCH 017/183] Transposed matrices so that they're ordered columnwise (instead of rowwise) and added some more type definitions (to make code faster). Still slow when it reaches new lap. Trying to make it faster by reducing the number of vectors created during coeffConstraintCost. --- eval_sim/eval_sim.jl | 4 + workspace/src/barc/src/LMPC_node.jl | 34 +++-- workspace/src/barc/src/helper/classes.jl | 30 ++-- .../barc/src/helper/coeffConstraintCost.jl | 131 ++++++++---------- workspace/src/barc/src/helper/createModel.jl | 6 +- .../src/barc/src/helper/solveMpcProblem.jl | 30 ++-- 6 files changed, 114 insertions(+), 121 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 778c2ea4..c9fa7c00 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -26,6 +26,10 @@ function eval_sim() plot(z.t,z.z[:,3],imu_meas.t,imu_meas.z,est.t,est.z[:,3]) grid(1) legend(["Real psi","psi meas","estimate"]) + figure() + plot(z.t,z.z[:,4]) + grid() + legend(["Velocity"]) end function eval_LMPC() diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index d804465a..6dfea4ec 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -72,11 +72,11 @@ function main() mpcParams.R = 0.0*[1 1] # cost matrix for control inputs mpcParams.Q = [0.0 10.0 1.0 1.0] # put weights on ey, epsi and v - oldTraj.oldTraj = zeros(4,buffersize,2) - oldTraj.oldInput = zeros(2,buffersize,2) + oldTraj.oldTraj = zeros(buffersize,4,2) + oldTraj.oldInput = zeros(buffersize,2,2) - mpcCoeff.coeffCost = 0 - mpcCoeff.coeffConst = 0 + mpcCoeff.coeffCost = Float64[] + mpcCoeff.coeffConst = Float64[] mpcCoeff.order = 5 mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon @@ -91,6 +91,10 @@ function main() zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information + # Precompile functions by running them once: + solve(mdl) + coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + println("Starting loop") while ! is_shutdown() if z_est[1] > 0 # check if data has been received (s > 0) @@ -114,10 +118,10 @@ function main() costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line # Save all data in oldTrajectory: if lapStatus.currentLap == 1 # if it's the first lap - oldTraj.oldTraj[:,:,1] = zCurr_export' # ... just save everything - oldTraj.oldInput[:,:,1] = uCurr_export' - oldTraj.oldTraj[:,:,2] = zCurr_export' - oldTraj.oldInput[:,:,2] = uCurr_export' + oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldTraj[:,:,2] = zCurr_export + oldTraj.oldInput[:,:,2] = uCurr_export oldTraj.oldCost = [costLap,costLap] else # idea: always copy the new trajectory in the first array! if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second @@ -125,8 +129,8 @@ function main() oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input oldTraj.oldCost[2] = oldTraj.oldCost[1] end - oldTraj.oldTraj[:,:,1] = zCurr_export' # ... and write the new traj in the first - oldTraj.oldInput[:,:,1] = uCurr_export' + oldTraj.oldTraj[:,:,1] = zCurr_export # ... and write the new traj in the first + oldTraj.oldInput[:,:,1] = uCurr_export oldTraj.oldCost[1] = costLap end #println(size(save_oldTraj)) @@ -138,10 +142,10 @@ function main() lapStatus.currentLap += 1 # start next lap lapStatus.currentIt = 1 # reset current iteration switchLap = false - tt = toc() + tt = toq() println("======================================== NEXT LAP ========================================") - println("oldTraj.oldTraj[1,:,1]:") - println(oldTraj.oldTraj[1,:,1]) + println("oldTraj.oldTraj[:,1,1]:") + println(oldTraj.oldTraj[:,1,1]) elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger switchLap = true end @@ -160,13 +164,13 @@ function main() # Find coefficients for cost and constraints tic() mpcCoeff = coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) - tt = toc() + tt = toq() println("Finished coefficients, t = $tt s") #println("Found coefficients: mpcCoeff = $mpcCoeff") # Solve the MPC problem tic() mpcSol = solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - tt = toc() + tt = toq() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] #println("trackCoeff = $trackCoeff") diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index a2c396b7..f6c7a117 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -12,32 +12,32 @@ end # 4th dimension specifies one of the two lap numbers between which are iterated type MpcCoeff # coefficients for trajectory approximation - coeffCost - coeffConst - order - pLength # small values here may lead to numerical problems since the functions are only approximated in a short horizon + coeffCost::Array{Float64} + coeffConst::Array{Float64} + order::Int64 + pLength::Int64 # small values here may lead to numerical problems since the functions are only approximated in a short horizon # "small" values are about 2*N, good values about 4*N # numerical problems occur at the edges (s=0, when v is almost 0 and s does not change fast and at s=s_target) - MpcCoeff(coeffCost=0, coeffConst=0, order=4, pLength=0) = new(coeffCost, coeffConst, order, pLength) + MpcCoeff(coeffCost=Float64[], coeffConst=Float64[], order=4, pLength=0) = new(coeffCost, coeffConst, order, pLength) end type OldTrajectory # information about previous trajectories oldTraj::Array{Float64} oldInput::Array{Float64} oldCost::Array{Int64} - OldTrajectory(oldTraj=zeros(100,4,2),oldInput=zeros(100,2,2),oldCost=[0,0]) = new(oldTraj,oldInput,oldCost) + OldTrajectory(oldTraj=Float64[],oldInput=Float64[],oldCost=Float64[]) = new(oldTraj,oldInput,oldCost) end type MpcParams # parameters for MPC solver N::Int64 nz::Int64 OrderCostCons::Int64 - Q - R + Q::Array{Float64} + R::Array{Float64} vPathFollowing::Float64 - QderivZ - QderivU - MpcParams(N=0,nz=0,OrderCostCons=0,Q=0,R=0,vPathFollowing=1.0,QderivZ=0.0,QderivU=0.0) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) + QderivZ::Array{Float64} + QderivU::Array{Float64} + MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[]) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) end type PosInfo # current position information @@ -51,10 +51,10 @@ type MpcSol # MPC solution output a_x::Float64 d_f::Float64 solverStatus - u - z - cost - MpcSol(a_x=0,d_f=0,solverStatus=0,u=0,z=0,cost=0) = new(a_x,d_f,solverStatus,u,z,cost) + u::Array{Float64} + z::Array{Float64} + cost::Array{Float64} + MpcSol(a_x=0.0,d_f=0.0,solverStatus=0,u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) end type TrackCoeff # coefficients of track diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index ed27f5e1..9f0333f6 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -16,9 +16,10 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # Outputs: # coeffConst # coeffCost + mpcCoeff::MpcCoeff # Read Inputs - tic() + #tic() oldTrajectory = oldTraj.oldTraj # [:,:,1] = 1st, [:,:,2] = 2nd oldInput = oldTraj.oldInput @@ -37,23 +38,21 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo pLength = mpcCoeff.pLength # interpolation length for polynomials - coeffCost = zeros(Order+1,1,2) # polynomial coefficients for cost - coeffConst = zeros(nz-1,Order+1,1,2) # nz-1 beacuse no coeff for s + coeffCost = zeros(Order+1,2) # polynomial coefficients for cost + coeffConst = zeros(Order+1,2,3) # nz-1 beacuse no coeff for s # Select the old data - oldS = oldTrajectory[1,:,:] - oldeY = oldTrajectory[2,:,:] - oldePsi = oldTrajectory[3,:,:] - oldV = oldTrajectory[4,:,:] + oldS = oldTrajectory[:,1,:] + oldeY = oldTrajectory[:,2,:] + oldePsi = oldTrajectory[:,3,:] + oldV = oldTrajectory[:,4,:] - N_points = size(oldTrajectory,2) # second dimension = length + N_points = size(oldTrajectory,1) # second dimension = length if lapNumber > 1 - tt=toc() - println("Loading parameters: $tt") - tic() - deltaOld = oldInput[1,:,:] - aOld = oldInput[2,:,:] + # tt=toq() + # println("Loading parameters: $tt") + # tic() # Compute the total s (current position along track) s_total = (s_start + s) % s_target @@ -61,27 +60,32 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # Compute the index DistS = ( s_total - oldS ).^2 - idx_s = findmin(DistS,2)[2] # contains both indices for the closest distances for both oldS !! + idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! - vec_range = (idx_s[1]:min(idx_s[1]+pLength,N_points),idx_s[2]:min(idx_s[2]+pLength,N_points*2)) - if idx_s[1]+pLength > N_points || idx_s[2]+pLength > 2*N_points - warn("Out of range!") - println("vec_range = $vec_range") - println("idx_s = $idx_s") - println("s_total = $s_total") - end + vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) + # if idx_s[1]+pLength > N_points || idx_s[2]+pLength > 2*N_points + # warn("Out of range!") + # println("vec_range = $vec_range") + # println("idx_s = $idx_s") + # println("s_total = $s_total") + # end # Create the vectors used for the interpolation # **************** WHAT IF MINIMUM INDEX + PLENGTH IS LONGER THAN ENTIRE OLD TRAJECTORY ? ******************************* # -> oldTrajectory is designed to go way beyond the "real" measured limit - bS_Vector = cat(3, oldS[vec_range[1]], oldS[vec_range[2]]) - beY_Vector = cat(3, oldeY[vec_range[1]], oldeY[vec_range[2]]) - bePsi_Vector = cat(3, oldePsi[vec_range[1]], oldePsi[vec_range[2]]) - bV_Vector = cat(3, oldV[vec_range[1]], oldV[vec_range[2]]) - - tt = toc() - println("2nd: $tt") - tic() + bS_Vector = zeros(pLength+1,2) + beY_Vector = zeros(pLength+1,2) + bePsi_Vector = zeros(pLength+1,2) + bV_Vector = zeros(pLength+1,2) + + bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) + beY_Vector = cat(2, oldeY[vec_range[1]], oldeY[vec_range[2]]) + bePsi_Vector = cat(2, oldePsi[vec_range[1]], oldePsi[vec_range[2]]) + bV_Vector = cat(2, oldV[vec_range[1]], oldV[vec_range[2]]) + + # tt = toq() + # println("2nd: $tt") + # tic() println("************************************** COEFFICIENTS **************************************") println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") println("s_total = $s_total") @@ -100,29 +104,29 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo MatrixInterp = zeros(pLength+1,Order+1,2) for k = 0:Order - MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,1,:].^k + MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k end # Compute the coefficients - CoefficientsFor_ey = zeros(Order+1,1,2) - CoefficientsFor_ePsi = zeros(Order+1,1,2) - CoefficientsFor_V = zeros(Order+1,1,2) + CoefficientsFor_ey = zeros(Order+1,2) + CoefficientsFor_ePsi = zeros(Order+1,2) + CoefficientsFor_V = zeros(Order+1,2) println("Calculating coefficients...") for i=1:2 - CoefficientsFor_ey[:,:,i] = MatrixInterp[:,:,i]\beY_Vector[:,:,i] - CoefficientsFor_ePsi[:,:,i] = MatrixInterp[:,:,i]\bePsi_Vector[:,:,i] - CoefficientsFor_V[:,:,i] = MatrixInterp[:,:,i]\bV_Vector[:,:,i] + CoefficientsFor_ey[:,i] = MatrixInterp[:,:,i]\beY_Vector[:,i] + CoefficientsFor_ePsi[:,i] = MatrixInterp[:,:,i]\bePsi_Vector[:,i] + CoefficientsFor_V[:,i] = MatrixInterp[:,:,i]\bV_Vector[:,i] end println("Done.") # Stack the coefficients - coeffConst = zeros(3,Order+1,1,2) - coeffConst[1,:,:,:] = CoefficientsFor_ey - coeffConst[2,:,:,:] = CoefficientsFor_ePsi - coeffConst[3,:,:,:] = CoefficientsFor_V - tt = toc() - println("3rd: $tt") - tic() + coeffConst = zeros(Order+1,2,3) + coeffConst[:,:,1] = CoefficientsFor_ey + coeffConst[:,:,2] = CoefficientsFor_ePsi + coeffConst[:,:,3] = CoefficientsFor_V + #tt = toq() + #println("3rd: $tt") + #tic() # structure of coeffConst: # 1st dimension specifies state # 2nd dimension specifies steps @@ -140,31 +144,10 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo for i=1:2 Qfunction = zeros(N_points,1) IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position - idx_s_target = find(oldS[1,:,i].>s_target)[1] + idx_s_target = find(oldS[:,i].>s_target)[1] dist_to_s_target = (idx_s_target - IndexBezierS) - dist_to_s_target = dist_to_s_target + 30 # set distance to finish line a bit higher - # for k=1:N_points - # # Start from the end ---> reverse the index - # indx = N_points-k+1 - - # # Here the if for the cost, minimize the distance to a straight line - # if oldS[1,indx,i] <= s_target - # QMatrix = 0.1 - # else - # QMatrix = 0 - # end - - # # Here actually computing the cost - # if (indx >= IndexBezierS) - # if k == 1 - # # If last point --> No Input - # Qfunction[indx] = QMatrix # [1] to transform 1-element-array to scalar - # else - # Qfunction[indx] = Qfunction[indx + 1] + QMatrix - # end - # end - # end - # NEEDS TO BE IMPROVED ************************** + #dist_to_s_target = dist_to_s_target + 30 + bQfunction_Vector = zeros(pLength+1,1) # Select the part needed for the interpolation #bQfunction_Vector = Qfunction[IndexBezierS:IndexBezierS+pLength] @@ -179,7 +162,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo #readline() # Compute coefficient for the cost - coeffCost[:,1,i] = MatrixInterp[:,:,i]\bQfunction_Vector + coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector # if maximum(coeffCost) > 1e4 # warn("Large coefficients in cost, might cause numerical problems.") # s = s_forinterpy[:,1,i] @@ -191,8 +174,8 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # end end - tt = toc() - println("4th: $tt") + #tt = toq() + #println("4th: $tt") # if maximum(coeffConst) > 1e4 # warn("Large coefficients in constraints, might cause numerical problems.") @@ -209,8 +192,10 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo else # if it is the first lap - coeffCost = zeros(Order+1,1,2) - coeffConst = zeros(nz-1,Order+1,1,2) # nz-1 because no coeff for s + coeffCost = zeros(Order+1,2) + coeffConst = zeros(Order+1,2,3) # nz-1 because no coeff for s end - return MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) + mpcCoeff = MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) + return mpcCoeff + #return MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) end diff --git a/workspace/src/barc/src/helper/createModel.jl b/workspace/src/barc/src/helper/createModel.jl index c80999e6..89dabadf 100644 --- a/workspace/src/barc/src/helper/createModel.jl +++ b/workspace/src/barc/src/helper/createModel.jl @@ -1,7 +1,7 @@ # Define Variables -trackCoeff = TrackCoeff() # info about track (at current position, approximated) -modelParams = ModelParams() -mpcParams = MpcParams() +trackCoeff = TrackCoeff() # info about track (at current position, approximated) +const modelParams = ModelParams() +const mpcParams = MpcParams() # =============================== # Initialize Parameters diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 8ed80478..2a7109a3 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -7,9 +7,11 @@ # i = 3 -> epsi # i = 4 -> v -function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr,uCurr) +function solveMpcProblem(mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr,uCurr) # Load Parameters + mpcSol::MpcSol + coeffCurvature = trackCoeff.coeffCurvature N = mpcParams.N Q = mpcParams.Q @@ -21,8 +23,6 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa s_target = posInfo.s_target ey_max = trackCoeff.width/2 - #zCurr[1] = posInfo.s # current relative s (used in interpolation) - QderivZ = mpcParams.QderivZ QderivU = mpcParams.QderivU @@ -37,8 +37,8 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa println("s_total = $((zCurr[1]+s_start)%s_target)") # Create function-specific parameters - z_Ref = cat(1,s_target*ones(1,N+1),zeros(2,N+1),v_ref*ones(1,N+1)) # Reference trajectory: path following -> stay on line and keep constant velocity - u_Ref = zeros(2,N) + z_Ref = cat(2,s_target*ones(N+1,1),zeros(N+1,2),v_ref*ones(N+1,1)) # Reference trajectory: path following -> stay on line and keep constant velocity + u_Ref = zeros(N,2) # Update current initial condition setvalue(z0,zCurr) @@ -62,18 +62,18 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa # Control Input cost # --------------------------------- - @NLexpression(mdl, controlCost, 0.5*sum{R[j]*sum{(u_Ol[j,i]-u_Ref[j,i])^2,i=1:N},j=1:2}) + @NLexpression(mdl, controlCost, 0.5*sum{R[j]*sum{(u_Ol[j,i]-u_Ref[i,j])^2,i=1:N},j=1:2}) # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap # termStateErr = (ParInt*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + # + (1 - ParInt)*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl, constZTerm, 10*(sum{(ParInt*sum{coeffTermConst[j,i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermConst[j,i,1,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3})) + @NLexpression(mdl, constZTerm, 10*(sum{(ParInt*sum{coeffTermConst[i,1,j]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermConst[i,2,j]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl, constZTerm, 10*sum{(sum{coeffTermConst[j,i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3}) + @NLexpression(mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3}) end # Terminal cost @@ -81,20 +81,20 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa if lapStatus.currentLap > 2 # if at least in the 3rd lap # costZTerm = ParInt*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + # + (1-ParInt)*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl, costZTerm, ParInt*sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermCost[i,1,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl, costZTerm, ParInt*sum{coeffTermCost[i,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermCost[i,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) # line above not necessary since the polynomial goes to zero anyways! elseif lapStatus.currentLap == 2 # if we're in the second second lap # costZTerm = polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl, costZTerm, sum{coeffTermCost[i,1,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl, costZTerm, sum{coeffTermCost[i,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) end # State cost # --------------------------------- if lapStatus.currentLap == 1 # if we're in the first lap, just do path following - @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[i,j]-z_Ref[i,j])^2,j=1:N+1},i=1:4}) # Follow trajectory + @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[i,j]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) #@NLexpression(mdl, costZ_h, 0) # zero state cost after crossing the finish line @@ -108,8 +108,8 @@ function solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelPa # Solve Problem and return solution sol_status = solve(mdl) sol_u = getvalue(u_Ol) - mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(u_Ol),getvalue(z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) - #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging + #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(u_Ol),getvalue(z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) + mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(z_Ol[1,N+1])) #println(getvalue(constZTerm)) From 1ec68f33c84ae0937e116e91980fbb736a7429a3 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 16 Sep 2016 08:52:21 -0700 Subject: [PATCH 018/183] reduced number of matrices in coeffConstCost to make calculations faster. next step: split up in multiple separate functions to facilitate compilation. --- eval_sim/eval_sim.jl | 2 +- workspace/src/barc/src/LMPC_node.jl | 21 +++++-- .../barc/src/helper/coeffConstraintCost.jl | 61 ++----------------- 3 files changed, 23 insertions(+), 61 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index c9fa7c00..c957b625 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -9,7 +9,7 @@ end log_path = "$(homedir())/simulations/output.jld" -log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" +log_path_LMPC = "$(homedir())/simulations/LMPC_output.jld" function eval_sim() d = load(log_path) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 6dfea4ec..ad544089 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -10,7 +10,7 @@ using data_service.msg using geometry_msgs.msg using JuMP using Ipopt -#using JLD +using JLD include("helper/classes.jl") include("helper/coeffConstraintCost.jl") @@ -58,7 +58,7 @@ function main() buffersize = 700 # Create data to be saved - # save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps + save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps # Define and initialize variables global mpcParams, trackCoeff, modelParams, z_est, posInfo, coeffCurvature_update, s_start_update @@ -91,6 +91,9 @@ function main() zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information + zCurr_export = zeros(buffersize,4) + uCurr_export = zeros(buffersize,2) + # Precompile functions by running them once: solve(mdl) coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) @@ -110,6 +113,7 @@ function main() # ======================================= Lap trigger ======================================= + # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... # ... then select and save data tic() @@ -136,6 +140,8 @@ function main() #println(size(save_oldTraj)) #println(size(oldTraj.oldTraj)) #save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state uCurr[1,:] = uCurr[i+1,:] # ... and input i = 1 @@ -143,9 +149,14 @@ function main() lapStatus.currentIt = 1 # reset current iteration switchLap = false tt = toq() + println("======================================== NEXT LAP ========================================") + println("Saved data, t = $tt") + println("cost: $(oldTraj.oldCost)") println("oldTraj.oldTraj[:,1,1]:") println(oldTraj.oldTraj[:,1,1]) + println("oldTraj.oldTraj[:,1,2]:") + println(oldTraj.oldTraj[:,1,2]) elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger switchLap = true end @@ -188,9 +199,9 @@ function main() rossleep(loop_rate) end # Save simulation data to file - # log_path = "$(homedir())/simulations/LMPC_output.jld" - # save(log_path,"oldTraj",save_oldTraj) - # println("Exiting LMPC node. Saved data.") + log_path = "$(homedir())/simulations/output_LMPC.jld" + save(log_path,"oldTraj",save_oldTraj) + println("Exiting LMPC node. Saved data.") end if ! isinteractive() diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index 9f0333f6..ed53e27a 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -63,25 +63,10 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) - # if idx_s[1]+pLength > N_points || idx_s[2]+pLength > 2*N_points - # warn("Out of range!") - # println("vec_range = $vec_range") - # println("idx_s = $idx_s") - # println("s_total = $s_total") - # end - + # Create the vectors used for the interpolation - # **************** WHAT IF MINIMUM INDEX + PLENGTH IS LONGER THAN ENTIRE OLD TRAJECTORY ? ******************************* - # -> oldTrajectory is designed to go way beyond the "real" measured limit bS_Vector = zeros(pLength+1,2) - beY_Vector = zeros(pLength+1,2) - bePsi_Vector = zeros(pLength+1,2) - bV_Vector = zeros(pLength+1,2) - bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) - beY_Vector = cat(2, oldeY[vec_range[1]], oldeY[vec_range[2]]) - bePsi_Vector = cat(2, oldePsi[vec_range[1]], oldePsi[vec_range[2]]) - bV_Vector = cat(2, oldV[vec_range[1]], oldV[vec_range[2]]) # tt = toq() # println("2nd: $tt") @@ -108,30 +93,18 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo end # Compute the coefficients - CoefficientsFor_ey = zeros(Order+1,2) - CoefficientsFor_ePsi = zeros(Order+1,2) - CoefficientsFor_V = zeros(Order+1,2) println("Calculating coefficients...") + coeffConst = zeros(Order+1,2,3) for i=1:2 - CoefficientsFor_ey[:,i] = MatrixInterp[:,:,i]\beY_Vector[:,i] - CoefficientsFor_ePsi[:,i] = MatrixInterp[:,:,i]\bePsi_Vector[:,i] - CoefficientsFor_V[:,i] = MatrixInterp[:,:,i]\bV_Vector[:,i] + coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] + coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] + coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldV[vec_range[i]] end println("Done.") - # Stack the coefficients - coeffConst = zeros(Order+1,2,3) - coeffConst[:,:,1] = CoefficientsFor_ey - coeffConst[:,:,2] = CoefficientsFor_ePsi - coeffConst[:,:,3] = CoefficientsFor_V #tt = toq() #println("3rd: $tt") #tic() - # structure of coeffConst: - # 1st dimension specifies state - # 2nd dimension specifies steps - # 3rd dimension not used - # 4th dimension specifies lapNumber # Finished with calculating the constraint coefficients @@ -146,6 +119,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position idx_s_target = find(oldS[:,i].>s_target)[1] dist_to_s_target = (idx_s_target - IndexBezierS) + println("dist_to_s_target $i = $dist_to_s_target") #dist_to_s_target = dist_to_s_target + 30 bQfunction_Vector = zeros(pLength+1,1) @@ -163,34 +137,11 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # Compute coefficient for the cost coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector - # if maximum(coeffCost) > 1e4 - # warn("Large coefficients in cost, might cause numerical problems.") - # s = s_forinterpy[:,1,i] - # interp = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * coeffCost[:,1,i] - # println(coeffCost) - # plot(s,bQfunction_Vector,"-o",s,interp) - # grid() - # readline() - # end end #tt = toq() #println("4th: $tt") - # if maximum(coeffConst) > 1e4 - # warn("Large coefficients in constraints, might cause numerical problems.") - # println(coeffConst) - # subplot(311) - # plot(s_forinterpy[:,:,1],beY_Vector[:,:,1],"-o") - # subplot(312) - # plot(s_forinterpy[:,:,1],bePsi_Vector[:,:,1],"-o") - # subplot(313) - # plot(s_forinterpy[:,:,1],bV_Vector[:,:,1],"-o") - # grid() - # readline() - # end - - else # if it is the first lap coeffCost = zeros(Order+1,2) coeffConst = zeros(Order+1,2,3) # nz-1 because no coeff for s From 875df3a05604161fe2c457f4e363bc674b29cd55 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 16 Sep 2016 15:53:04 -0700 Subject: [PATCH 019/183] Added profiler LMPC function (just for simulation and profiling), added some more type definitions to make code faster. Stil working on it, next step: Get rid of global variables. --- eval_sim/eval_sim.jl | 8 + workspace/src/barc/src/LMPC_node.jl | 72 +++---- workspace/src/barc/src/LMPC_node_profiler.jl | 184 ++++++++++++++++ workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/helper/classes.jl | 30 +-- .../barc/src/helper/coeffConstraintCost.jl | 201 +++++++++--------- workspace/src/barc/src/helper/createModel.jl | 11 +- workspace/src/barc/src/helper/functions.jl | 31 +++ .../src/barc/src/helper/solveMpcProblem.jl | 22 +- 9 files changed, 390 insertions(+), 171 deletions(-) create mode 100644 workspace/src/barc/src/LMPC_node_profiler.jl create mode 100644 workspace/src/barc/src/helper/functions.jl diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index c957b625..d52f5083 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -1,5 +1,6 @@ using JLD using PyPlot +using HDF5, JLD, ProfileView type Measurements{T} i::Int64 # measurement counter @@ -10,6 +11,7 @@ end log_path = "$(homedir())/simulations/output.jld" log_path_LMPC = "$(homedir())/simulations/LMPC_output.jld" +log_path_profile = "$(homedir())/simulations/profile.jlprof" function eval_sim() d = load(log_path) @@ -37,4 +39,10 @@ function eval_LMPC() oldTraj = d["oldTraj"] plot(oldTraj[:,:,1,1]) grid(1) +end + +function eval_prof() + Profile.clear() + @load "$(homedir())/simulations/profile.jlprof" + ProfileView.view(li, lidict=lidict) end \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index ad544089..ba41ce51 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -16,6 +16,7 @@ include("helper/classes.jl") include("helper/coeffConstraintCost.jl") include("helper/solveMpcProblem.jl") include("helper/computeCostLap.jl") +include("helper/functions.jl") # Load Variables and create Model: println("Loading and defining variables...") @@ -26,19 +27,14 @@ println("Initial solve...") solve(mdl) println("Finished initial solve.") - +# Global variables: z_est = zeros(1,4) - - -posInfo = PosInfo() -posInfo.s_start = 0 -posInfo.s_target = 10.281192 coeffCurvature_update = 0 # use extra update variables so that they're not suddenly changed within functions s_start_update = 0 function SE_callback(msg::pos_info) # update current position and track data # update mpc initial condition - global z_est, posInfo, coeffCurvature_update, s_start_update + global z_est, coeffCurvature_update, s_start_update z_est = [msg.s msg.ey msg.epsi msg.v] s_start_update = msg.s_start coeffCurvature_update = msg.coeffCurvature @@ -61,23 +57,27 @@ function main() save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps # Define and initialize variables - global mpcParams, trackCoeff, modelParams, z_est, posInfo, coeffCurvature_update, s_start_update + global mpcParams, trackCoeff, modelParams, z_est, coeffCurvature_update, s_start_update oldTraj = OldTrajectory() + posInfo = PosInfo() mpcCoeff = MpcCoeff() lapStatus = LapStatus(1,1) mpcSol = MpcSol() - mpcParams.QderivZ = 0.0*[1 1 1 1] # cost matrix for derivative cost of states - mpcParams.QderivU = 0.1*[1 1] # cost matrix for derivative cost of inputs - mpcParams.R = 0.0*[1 1] # cost matrix for control inputs - mpcParams.Q = [0.0 10.0 1.0 1.0] # put weights on ey, epsi and v + posInfo.s_start = 0 + posInfo.s_target = 8.281192 + + mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs + mpcParams.R = 0.0*[1,1] # cost matrix for control inputs + mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) - mpcCoeff.coeffCost = Float64[] - mpcCoeff.coeffConst = Float64[] mpcCoeff.order = 5 + mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) + mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) # nz-1 because no coeff for s mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon lapStatus.currentLap = 1 # initialize lap number @@ -96,9 +96,9 @@ function main() # Precompile functions by running them once: solve(mdl) - coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + #coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + #precompile(saveOldTraj,(OldTrajectory,Array{Float64},Array{Float64},LapStatus,Int64,Float64)) - println("Starting loop") while ! is_shutdown() if z_est[1] > 0 # check if data has been received (s > 0) @@ -116,42 +116,20 @@ function main() # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... # ... then select and save data + println("Saving data") tic() - zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) - uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) - costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line - # Save all data in oldTrajectory: - if lapStatus.currentLap == 1 # if it's the first lap - oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything - oldTraj.oldInput[:,:,1] = uCurr_export - oldTraj.oldTraj[:,:,2] = zCurr_export - oldTraj.oldInput[:,:,2] = uCurr_export - oldTraj.oldCost = [costLap,costLap] - else # idea: always copy the new trajectory in the first array! - if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second - oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second - oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input - oldTraj.oldCost[2] = oldTraj.oldCost[1] - end - oldTraj.oldTraj[:,:,1] = zCurr_export # ... and write the new traj in the first - oldTraj.oldInput[:,:,1] = uCurr_export - oldTraj.oldCost[1] = costLap - end - #println(size(save_oldTraj)) - #println(size(oldTraj.oldTraj)) - #save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,dt) save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] - zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state uCurr[1,:] = uCurr[i+1,:] # ... and input i = 1 lapStatus.currentLap += 1 # start next lap lapStatus.currentIt = 1 # reset current iteration switchLap = false - tt = toq() - println("======================================== NEXT LAP ========================================") + tt = toq() println("Saved data, t = $tt") + println("======================================== NEXT LAP ========================================") println("cost: $(oldTraj.oldCost)") println("oldTraj.oldTraj[:,1,1]:") println(oldTraj.oldTraj[:,1,1]) @@ -174,13 +152,15 @@ function main() # Find coefficients for cost and constraints tic() - mpcCoeff = coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + if lapStatus.currentLap > 1 + coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + end tt = toq() println("Finished coefficients, t = $tt s") #println("Found coefficients: mpcCoeff = $mpcCoeff") # Solve the MPC problem tic() - mpcSol = solveMpcProblem(mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + solveMpcProblem(mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') tt = toq() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] @@ -199,11 +179,13 @@ function main() rossleep(loop_rate) end # Save simulation data to file + log_path = "$(homedir())/simulations/output_LMPC.jld" save(log_path,"oldTraj",save_oldTraj) println("Exiting LMPC node. Saved data.") + end if ! isinteractive() main() -end \ No newline at end of file +end diff --git a/workspace/src/barc/src/LMPC_node_profiler.jl b/workspace/src/barc/src/LMPC_node_profiler.jl new file mode 100644 index 00000000..9a98dfcd --- /dev/null +++ b/workspace/src/barc/src/LMPC_node_profiler.jl @@ -0,0 +1,184 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +include("helper/classes.jl") +include("helper/coeffConstraintCost.jl") +include("helper/solveMpcProblem.jl") +include("helper/computeCostLap.jl") +include("helper/functions.jl") + +# Load Variables and create Model: +println("Loading and defining variables...") +include("helper/createModel.jl") + +# Initialize model by solving it once +println("Initial solve...") +solve(mdl) +println("Finished initial solve.") + +# Global variables: +z_est = zeros(1,4) +coeffCurvature_update = zeros(5) # use extra update variables so that they're not suddenly changed within functions +s_start_update = 0.0 + + +function main() + println("now starting the node") + # initiate node, set up publisher / subscriber topics + #init_node("mpc_traj") + #loop_rate = Rate(10) + + buffersize = 700 + + # Create data to be saved + save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps + + # Define and initialize variables + global mpcParams, trackCoeff, modelParams, z_est, coeffCurvature_update, s_start_update + oldTraj = OldTrajectory() + posInfo = PosInfo() + mpcCoeff = MpcCoeff() + lapStatus = LapStatus(1,1) + mpcSol = MpcSol() + + posInfo.s_start = 0 + posInfo.s_target = 8.281192 + + mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs + mpcParams.R = 0.0*[1,1] # cost matrix for control inputs + mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v + + oldTraj.oldTraj = zeros(buffersize,4,2) + oldTraj.oldInput = zeros(buffersize,2,2) + + mpcCoeff.order = 5 + mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) + mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) # nz-1 because no coeff for s + mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon + + lapStatus.currentLap = 1 # initialize lap number + lapStatus.currentIt = 0 # current iteration in lap + + # Lap parameters + switchLap = false # initialize lap lap trigger + s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] + + # buffer in current lap + zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) + uCurr = zeros(10000,2) # contains input information + + zCurr_export = zeros(buffersize,4) + uCurr_export = zeros(buffersize,2) + + # Precompile functions by running them once: + solve(mdl) + #coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + precompile(saveOldTraj,(OldTrajectory,Array{Float64},Array{Float64},LapStatus,Int64,Float64)) + + z_sim = [0 0 0 0.5] + #while ! is_shutdown() + for j=1:300 + z_sim[1] += 0.05 + z_est = z_sim + if z_est[1] > 0 # check if data has been received (s > 0) + + # ============================= Initialize iteration parameters ============================= + lapStatus.currentIt += 1 # count iteration + + i = lapStatus.currentIt # current iteration number, just to make notation shorter + zCurr[i,:] = z_est # update state information + posInfo.s = z_est[1] # update position info + posInfo.s_start = s_start_update + trackCoeff.coeffCurvature = coeffCurvature_update + + + # ======================================= Lap trigger ======================================= + # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! + if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... + z_sim[1] = 0 + # ... then select and save data + println("Saving data") + tic() + saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,dt) + save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state + uCurr[1,:] = uCurr[i+1,:] # ... and input + i = 1 + lapStatus.currentLap += 1 # start next lap + lapStatus.currentIt = 1 # reset current iteration + switchLap = false + + tt = toq() + println("Saved data, t = $tt") + println("======================================== NEXT LAP ========================================") + println("cost: $(oldTraj.oldCost)") + println("oldTraj.oldTraj[:,1,1]:") + println(oldTraj.oldTraj[:,1,1]) + println("oldTraj.oldTraj[:,1,2]:") + println(oldTraj.oldTraj[:,1,2]) + elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger + switchLap = true + end + + + # ======================================= Calculate input ======================================= + println("======================================== NEW ITERATION # $i ========================================") + println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") + println("State Nr. $i = $z_est") + println("Coeff Curvature = $(trackCoeff.coeffCurvature)") + println("posInfo = $posInfo") + println("s = $(posInfo.s)") + println("s_start = $(posInfo.s_start)") + println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") + + # Find coefficients for cost and constraints + tic() + if lapStatus.currentLap > 1 + coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + end + tt = toq() + println("Finished coefficients, t = $tt s") + #println("Found coefficients: mpcCoeff = $mpcCoeff") + # Solve the MPC problem + tic() + solveMpcProblem(mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + tt = toq() + # Write in current input information + uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] + #println("trackCoeff = $trackCoeff") + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") + # ... and publish data + # cmd = ECU(mpcSol.a_x, mpcSol.d_f) + # publish(pub, cmd) + + zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) + println("\n") + + else + println("No estimation data received!") + end + #rossleep(loop_rate) + end + # Save simulation data to file + + log_path = "$(homedir())/simulations/output_LMPC.jld" + save(log_path,"oldTraj",save_oldTraj) + println("Exiting LMPC node. Saved data.") + +end + +if ! isinteractive() + main() +end diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 34fb7c94..cb522590 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) + l.create_racetrack(1.0,2.0,0.2,array([0.0,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index f6c7a117..92050758 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -32,8 +32,8 @@ type MpcParams # parameters for MPC solver N::Int64 nz::Int64 OrderCostCons::Int64 - Q::Array{Float64} - R::Array{Float64} + Q::Array{Float64,1} + R::Array{Float64,1} vPathFollowing::Float64 QderivZ::Array{Float64} QderivU::Array{Float64} @@ -50,29 +50,29 @@ end type MpcSol # MPC solution output a_x::Float64 d_f::Float64 - solverStatus + solverStatus::Int64 u::Array{Float64} z::Array{Float64} cost::Array{Float64} - MpcSol(a_x=0.0,d_f=0.0,solverStatus=0,u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) + MpcSol(a_x=0.0,d_f=0.0,solverStatus=1,u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) end type TrackCoeff # coefficients of track - coeffAngle - coeffCurvature - nPolyCurvature # order of the interpolation polynom - width # lane width -> is used in cost function as soft constraints (to stay on track) - TrackCoeff(coeffAngle=0,coeffCurvature=0,nPolyCurvature=4) = new(coeffAngle,coeffCurvature,nPolyCurvature) + coeffAngle::Array{Float64} + coeffCurvature::Array{Float64} + nPolyCurvature::Int64 # order of the interpolation polynom + width::Float64 # lane width -> is used in cost function as soft constraints (to stay on track) + TrackCoeff(coeffAngle=Float64[],coeffCurvature=Float64[],nPolyCurvature=4,width=1.0) = new(coeffAngle,coeffCurvature,nPolyCurvature) end type ModelParams l_A::Float64 l_B::Float64 dt::Float64 - u_lb # lower bounds for u - u_ub # upper bounds - z_lb - z_ub - c0 - ModelParams(l_A=0.25,l_B=0.25,dt=0.1,u_lb=0,u_ub=0,z_lb=0,z_ub=0,c0=0) = new(l_A,l_B,dt,u_lb,u_ub,z_lb,z_ub,c0) + u_lb::Array{Float64} # lower bounds for u + u_ub::Array{Float64} # upper bounds + z_lb::Array{Float64} + z_ub::Array{Float64} + c0::Array{Float64} + ModelParams(l_A=0.25,l_B=0.25,dt=0.1,u_lb=Float64[],u_ub=Float64[],z_lb=Float64[],z_ub=Float64[],c0=Float64[]) = new(l_A,l_B,dt,u_lb,u_ub,z_lb,z_ub,c0) end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index ed53e27a..8a47936a 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -16,13 +16,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # Outputs: # coeffConst # coeffCost - mpcCoeff::MpcCoeff # Read Inputs - #tic() - oldTrajectory = oldTraj.oldTraj # [:,:,1] = 1st, [:,:,2] = 2nd - oldInput = oldTraj.oldInput - + tic() lapNumber = lapStatus.currentLap s_start = posInfo.s_start @@ -42,111 +38,124 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo coeffConst = zeros(Order+1,2,3) # nz-1 beacuse no coeff for s # Select the old data - oldS = oldTrajectory[:,1,:] - oldeY = oldTrajectory[:,2,:] - oldePsi = oldTrajectory[:,3,:] - oldV = oldTrajectory[:,4,:] - - N_points = size(oldTrajectory,1) # second dimension = length - - if lapNumber > 1 - # tt=toq() - # println("Loading parameters: $tt") - # tic() - - # Compute the total s (current position along track) + oldS = oldTraj.oldTraj[:,1,:]::Array{Float64,3} + oldeY = oldTraj.oldTraj[:,2,:]::Array{Float64,3} + oldePsi = oldTraj.oldTraj[:,3,:]::Array{Float64,3} + oldV = oldTraj.oldTraj[:,4,:]::Array{Float64,3} + + N_points = size(oldTraj.oldTraj,1) # second dimension = length + + s_total::Float64 # initialize + DistS::Array{Float64} # initialize + idx_s::Array{Int64} # initialize + idx_s_target = 0 + dist_to_s_target = 0 + qLength = 0 + vec_range::Tuple{UnitRange{Int64},UnitRange{Int64}} + bS_Vector::Array{Float64} + s_forinterpy::Array{Float64} + + # Compute the total s (current position along track) s_total = (s_start + s) % s_target # Compute the index DistS = ( s_total - oldS ).^2 + + tt=toq() + println("Loading parameters: $tt") + tic() + idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) # Create the vectors used for the interpolation bS_Vector = zeros(pLength+1,2) - bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) - - # tt = toq() - # println("2nd: $tt") - # tic() - println("************************************** COEFFICIENTS **************************************") - println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") - println("s_total = $s_total") - # println("bS_Vector[1] = $(bS_Vector[:,:,1]')") - # These matrices (above) contain two vectors each (for both old trajectories), stored in the 3rd dimension - - # The states are parametrized with resprect to the curvilinear abscissa, - # so we select the point used for the interpolation. Need to subtract an - # offset to be coherent with the MPC formulation - s_forinterpy = bS_Vector - s_start - if s_total - s_start < 0 - s_forinterpy += s_target - end - println("s_forinterpy[:,1,1]' = $(s_forinterpy[:,1,1]')") - # Create the Matrices for the interpolation - MatrixInterp = zeros(pLength+1,Order+1,2) - - for k = 0:Order - MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k - end - - # Compute the coefficients - println("Calculating coefficients...") - coeffConst = zeros(Order+1,2,3) - for i=1:2 - coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] - coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] - coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldV[vec_range[i]] + for i=1:pLength+1 + bS_Vector[i,1] = oldS[vec_range[1][i]] + bS_Vector[i,2] = oldS[vec_range[2][i]] end - println("Done.") - - #tt = toq() - #println("3rd: $tt") - #tic() - - # Finished with calculating the constraint coefficients - - # Now compute the final cost coefficients - - # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value - # These values are calculated for both old trajectories - # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line - # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost - for i=1:2 - Qfunction = zeros(N_points,1) - IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position - idx_s_target = find(oldS[:,i].>s_target)[1] - dist_to_s_target = (idx_s_target - IndexBezierS) - println("dist_to_s_target $i = $dist_to_s_target") - #dist_to_s_target = dist_to_s_target + 30 - - bQfunction_Vector = zeros(pLength+1,1) - # Select the part needed for the interpolation - #bQfunction_Vector = Qfunction[IndexBezierS:IndexBezierS+pLength] - qLength = min(dist_to_s_target,pLength+1) - #println(bQfunction_Vector) - bQfunction_Vector = zeros(pLength+1,1) - bQfunction_Vector[1:qLength] = (dist_to_s_target:-1:dist_to_s_target-qLength+1)*0.1 - - #bQfunction_Vector = collect((dist_to_s_target:-1:dist_to_s_target-pLength))*0.1 - #println("length = $(length(bQfunction_Vector)), $(pLength+1)") - #println(bQfunction_Vector) - #readline() - - # Compute coefficient for the cost - coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector + # bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) + + tt = toq() + println("2nd: $tt") + tic() + println("************************************** COEFFICIENTS **************************************") + println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") + println("s_total = $s_total") + # println("bS_Vector[1] = $(bS_Vector[:,:,1]')") + # These matrices (above) contain two vectors each (for both old trajectories), stored in the 3rd dimension + + # The states are parametrized with resprect to the curvilinear abscissa, + # so we select the point used for the interpolation. Need to subtract an + # offset to be coherent with the MPC formulation + s_forinterpy = bS_Vector - s_start + if s_total - s_start < 0 + s_forinterpy += s_target + end + println("s_forinterpy[:,1,1]' = $(s_forinterpy[:,1,1]')") + # Create the Matrices for the interpolation + MatrixInterp = zeros(pLength+1,Order+1,2) - end - #tt = toq() - #println("4th: $tt") + for k = 0:Order + MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k + end + + # Compute the coefficients + println("Calculating coefficients...") + coeffConst = zeros(Order+1,2,3) + for i=1:2 + coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] + coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] + coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldV[vec_range[i]] + end + println("Done.") + + tt = toq() + println("3rd: $tt") + tic() + + # Finished with calculating the constraint coefficients + + # Now compute the final cost coefficients + + # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value + # These values are calculated for both old trajectories + # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line + # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost + for i=1:2 + Qfunction = zeros(N_points,1) + IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position + idx_s_target = find(oldS[:,i].>s_target)[1] + dist_to_s_target = (idx_s_target - IndexBezierS) + println("dist_to_s_target $i = $dist_to_s_target") + #dist_to_s_target = dist_to_s_target + 30 + + bQfunction_Vector = zeros(pLength+1,1) + # Select the part needed for the interpolation + #bQfunction_Vector = Qfunction[IndexBezierS:IndexBezierS+pLength] + qLength = min(dist_to_s_target,pLength+1) + #println(bQfunction_Vector) + bQfunction_Vector = zeros(pLength+1,1) + bQfunction_Vector[1:qLength] = (dist_to_s_target:-1:dist_to_s_target-qLength+1)*0.1 + + #bQfunction_Vector = collect((dist_to_s_target:-1:dist_to_s_target-pLength))*0.1 + #println("length = $(length(bQfunction_Vector)), $(pLength+1)") + #println(bQfunction_Vector) + #readline() + + # Compute coefficient for the cost + coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector - else # if it is the first lap - coeffCost = zeros(Order+1,2) - coeffConst = zeros(Order+1,2,3) # nz-1 because no coeff for s end - mpcCoeff = MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) - return mpcCoeff + tt = toq() + println("4th: $tt") + + mpcCoeff.coeffCost = coeffCost + mpcCoeff.coeffConst = coeffConst + #mpcCoeff = MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) + #return mpcCoeff #return MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) + nothing end diff --git a/workspace/src/barc/src/helper/createModel.jl b/workspace/src/barc/src/helper/createModel.jl index 89dabadf..11afc208 100644 --- a/workspace/src/barc/src/helper/createModel.jl +++ b/workspace/src/barc/src/helper/createModel.jl @@ -1,7 +1,7 @@ # Define Variables -trackCoeff = TrackCoeff() # info about track (at current position, approximated) -const modelParams = ModelParams() -const mpcParams = MpcParams() +trackCoeff = TrackCoeff() # info about track (at current position, approximated) +modelParams = ModelParams() +mpcParams = MpcParams() # =============================== # Initialize Parameters @@ -10,8 +10,9 @@ const mpcParams = MpcParams() mpcParams.N = 5 mpcParams.nz = 4 -mpcParams.Q = [0.0 1.0 1.0 1.0] # put weights on ey, epsi and v -mpcParams.vPathFollowing = 0.2 +mpcParams.Q = [0.0,1.0,1.0,1.0] # put weights on ey, epsi and v +mpcParams.R = [0.0,0.0] # put weights on ey, epsi and v +mpcParams.vPathFollowing = 0.5 trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl new file mode 100644 index 00000000..54607528 --- /dev/null +++ b/workspace/src/barc/src/helper/functions.jl @@ -0,0 +1,31 @@ +function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{Float64},lapStatus::LapStatus,buffersize::Int64,dt::Float64) + + i = lapStatus.currentIt # current iteration number, just to make notation shorter + zCurr_export = zeros(buffersize,4) + uCurr_export = zeros(buffersize,2) + zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) + uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) + costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line + # Save all data in oldTrajectory: + if lapStatus.currentLap == 1 # if it's the first lap + oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldTraj[:,:,2] = zCurr_export + oldTraj.oldInput[:,:,2] = uCurr_export + oldTraj.oldCost = [costLap,costLap] + else # idea: always copy the new trajectory in the first array! + if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second + oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second + oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input + oldTraj.oldCost[2] = oldTraj.oldCost[1] + end + oldTraj.oldTraj[:,:,1] = zCurr_export # ... and write the new traj in the first + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldCost[1] = costLap + end + #println(size(save_oldTraj)) + #println(size(oldTraj.oldTraj)) + #save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + + +end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 2a7109a3..53a9f71e 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -7,27 +7,28 @@ # i = 3 -> epsi # i = 4 -> v -function solveMpcProblem(mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr,uCurr) +function solveMpcProblem(mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uCurr::Array{Float64}) # Load Parameters - mpcSol::MpcSol - coeffCurvature = trackCoeff.coeffCurvature + coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} N = mpcParams.N Q = mpcParams.Q R = mpcParams.R - coeffTermCost = mpcCoeff.coeffCost - coeffTermConst = mpcCoeff.coeffConst + coeffTermCost = mpcCoeff.coeffCost::Array{Float64,2} + coeffTermConst = mpcCoeff.coeffConst::Array{Float64,3} order = mpcCoeff.order # polynomial order of terminal constraints and cost approximation s_start = posInfo.s_start s_target = posInfo.s_target ey_max = trackCoeff.width/2 - QderivZ = mpcParams.QderivZ - QderivU = mpcParams.QderivU + QderivZ = mpcParams.QderivZ::Array{Float64,1} + QderivU = mpcParams.QderivU::Array{Float64,1} v_ref = mpcParams.vPathFollowing + sol_u::Array{Float64,2} + global mdl println("************************************** MPC SOLVER **************************************") @@ -37,6 +38,7 @@ function solveMpcProblem(mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::Tra println("s_total = $((zCurr[1]+s_start)%s_target)") # Create function-specific parameters + z_Ref::Array{Float64,2} z_Ref = cat(2,s_target*ones(N+1,1),zeros(N+1,2),v_ref*ones(N+1,1)) # Reference trajectory: path following -> stay on line and keep constant velocity u_Ref = zeros(N,2) @@ -109,7 +111,9 @@ function solveMpcProblem(mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::Tra sol_status = solve(mdl) sol_u = getvalue(u_Ol) #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(u_Ol),getvalue(z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) - mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging + mpcSol.a_x = sol_u[1,1] + mpcSol.d_f = sol_u[2,1] + #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(z_Ol[1,N+1])) #println(getvalue(constZTerm)) @@ -128,5 +132,5 @@ function solveMpcProblem(mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::Tra # println(getvalue(z_Ol)) # println("==============") # println(getvalue(u_Ol)) - return mpcSol + nothing end From 192906a8741a1bd5d644155c90c9a819331a87ca Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 16 Sep 2016 16:25:38 -0700 Subject: [PATCH 020/183] Changed subscriber so that it writes into passed arguments instead of using global variables. --- workspace/src/barc/src/LMPC_node.jl | 39 +++++++++++++----------- workspace/src/barc/src/helper/classes.jl | 4 +-- 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index ba41ce51..f3f514b1 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -27,28 +27,19 @@ println("Initial solve...") solve(mdl) println("Finished initial solve.") -# Global variables: -z_est = zeros(1,4) -coeffCurvature_update = 0 # use extra update variables so that they're not suddenly changed within functions -s_start_update = 0 -function SE_callback(msg::pos_info) # update current position and track data +function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1}) # update current position and track data # update mpc initial condition - global z_est, coeffCurvature_update, s_start_update - z_est = [msg.s msg.ey msg.epsi msg.v] - s_start_update = msg.s_start - coeffCurvature_update = msg.coeffCurvature - #println("Received pos info: $msg") - #println("coeffCurvature = $(trackCoeff.coeffCurvature)") + z_est[:] = [msg.s,msg.ey,msg.epsi,msg.v] # use z_est as pointer + s_start_update[1] = msg.s_start + coeffCurvature_update[:] = msg.coeffCurvature + println("Updated values.") end function main() println("now starting the node") # initiate node, set up publisher / subscriber topics init_node("mpc_traj") - pub = Publisher("ecu", ECU, queue_size=10) - pub2 = Publisher("logging", Logging, queue_size=10) - s1 = Subscriber("pos_info", pos_info, SE_callback, queue_size=10) loop_rate = Rate(10) buffersize = 700 @@ -57,12 +48,23 @@ function main() save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps # Define and initialize variables - global mpcParams, trackCoeff, modelParams, z_est, coeffCurvature_update, s_start_update + global mpcParams, trackCoeff, modelParams oldTraj = OldTrajectory() posInfo = PosInfo() mpcCoeff = MpcCoeff() lapStatus = LapStatus(1,1) mpcSol = MpcSol() + + + z_est = zeros(4) + coeffCurvature_update = zeros(5) + s_start_update = [0.0] + + pub = Publisher("ecu", ECU, queue_size=10) + pub2 = Publisher("logging", Logging, queue_size=10) + + # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: + s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) posInfo.s_start = 0 posInfo.s_target = 8.281192 @@ -100,7 +102,10 @@ function main() #precompile(saveOldTraj,(OldTrajectory,Array{Float64},Array{Float64},LapStatus,Int64,Float64)) while ! is_shutdown() - if z_est[1] > 0 # check if data has been received (s > 0) + if z_est[1] > 0 # check if data has been received (s > 0) + println("Received data: z_est = $z_est") + println("curvature = $coeffCurvature_update") + println("s_start = $s_start_update") # ============================= Initialize iteration parameters ============================= lapStatus.currentIt += 1 # count iteration @@ -108,7 +113,7 @@ function main() i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = z_est # update state information posInfo.s = z_est[1] # update position info - posInfo.s_start = s_start_update + posInfo.s_start = s_start_update[1] trackCoeff.coeffCurvature = coeffCurvature_update diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index 92050758..1ddd31c9 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -58,8 +58,8 @@ type MpcSol # MPC solution output end type TrackCoeff # coefficients of track - coeffAngle::Array{Float64} - coeffCurvature::Array{Float64} + coeffAngle::Array{Float64,1} + coeffCurvature::Array{Float64,1} nPolyCurvature::Int64 # order of the interpolation polynom width::Float64 # lane width -> is used in cost function as soft constraints (to stay on track) TrackCoeff(coeffAngle=Float64[],coeffCurvature=Float64[],nPolyCurvature=4,width=1.0) = new(coeffAngle,coeffCurvature,nPolyCurvature) From 0ae8cdeb67cc80d02e12e2416bd99388cc36a586 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 16 Sep 2016 20:22:11 -0700 Subject: [PATCH 021/183] Changed entire problem formulation structure so that there are no more global variables. Instead, the model is defined by a function and passed to the MPC-solving function. Still not solving perfectly, but getting there. --- workspace/src/barc/src/LMPC_node.jl | 85 +++++++---------- workspace/src/barc/src/LMPC_node_profiler.jl | 93 +++++++++---------- workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/helper/classes.jl | 43 ++++++++- workspace/src/barc/src/helper/createModel.jl | 83 ----------------- workspace/src/barc/src/helper/functions.jl | 85 +++++++++++++++++ .../src/barc/src/helper/solveMpcProblem.jl | 89 ++++++++++-------- 7 files changed, 254 insertions(+), 226 deletions(-) delete mode 100644 workspace/src/barc/src/helper/createModel.jl diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index f3f514b1..47a29152 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -18,22 +18,11 @@ include("helper/solveMpcProblem.jl") include("helper/computeCostLap.jl") include("helper/functions.jl") -# Load Variables and create Model: -println("Loading and defining variables...") -include("helper/createModel.jl") - -# Initialize model by solving it once -println("Initial solve...") -solve(mdl) -println("Finished initial solve.") - - function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1}) # update current position and track data # update mpc initial condition z_est[:] = [msg.s,msg.ey,msg.epsi,msg.v] # use z_est as pointer s_start_update[1] = msg.s_start coeffCurvature_update[:] = msg.coeffCurvature - println("Updated values.") end function main() @@ -48,58 +37,52 @@ function main() save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps # Define and initialize variables - global mpcParams, trackCoeff, modelParams oldTraj = OldTrajectory() posInfo = PosInfo() mpcCoeff = MpcCoeff() lapStatus = LapStatus(1,1) mpcSol = MpcSol() - + trackCoeff = TrackCoeff() # info about track (at current position, approximated) + modelParams = ModelParams() + mpcParams = MpcParams() - z_est = zeros(4) - coeffCurvature_update = zeros(5) - s_start_update = [0.0] - - pub = Publisher("ecu", ECU, queue_size=10) - pub2 = Publisher("logging", Logging, queue_size=10) + z_est = zeros(4) + coeffCurvature_update = zeros(5) + s_start_update = [0.0] + cmd = ECU(0.0,0.0) + pub = Publisher("ecu", ECU, queue_size=10) + pub2 = Publisher("logging", Logging, queue_size=10) # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) - - posInfo.s_start = 0 - posInfo.s_target = 8.281192 - - mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs - mpcParams.R = 0.0*[1,1] # cost matrix for control inputs - mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v - - oldTraj.oldTraj = zeros(buffersize,4,2) - oldTraj.oldInput = zeros(buffersize,2,2) - - mpcCoeff.order = 5 - mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) - mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) # nz-1 because no coeff for s - mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon - - lapStatus.currentLap = 1 # initialize lap number - lapStatus.currentIt = 0 # current iteration in lap + s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) + InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + println("Finished initialization.") # Lap parameters - switchLap = false # initialize lap lap trigger - s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] + switchLap = false # initialize lap lap trigger + s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] # buffer in current lap - zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) - uCurr = zeros(10000,2) # contains input information + zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) + uCurr = zeros(10000,2) # contains input information + + zCurr_export = zeros(buffersize,4) + uCurr_export = zeros(buffersize,2) + + + # DEFINE MODEL *************************************************************************** + # **************************************************************************************** + println("Building model...") + + z_Init = zeros(4) - zCurr_export = zeros(buffersize,4) - uCurr_export = zeros(buffersize,2) + mdl = MpcModel() + InitializeModel(mdl,mpcParams,modelParams,trackCoeff,z_Init) - # Precompile functions by running them once: - solve(mdl) - #coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) - #precompile(saveOldTraj,(OldTrajectory,Array{Float64},Array{Float64},LapStatus,Int64,Float64)) + # Initial solve: + println("Initial solve...") + solve(mdl.mdl) + println("Finished.") while ! is_shutdown() if z_est[1] > 0 # check if data has been received (s > 0) @@ -123,7 +106,7 @@ function main() # ... then select and save data println("Saving data") tic() - saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,dt) + saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state uCurr[1,:] = uCurr[i+1,:] # ... and input @@ -165,7 +148,7 @@ function main() #println("Found coefficients: mpcCoeff = $mpcCoeff") # Solve the MPC problem tic() - solveMpcProblem(mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') tt = toq() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] diff --git a/workspace/src/barc/src/LMPC_node_profiler.jl b/workspace/src/barc/src/LMPC_node_profiler.jl index 9a98dfcd..66506e64 100644 --- a/workspace/src/barc/src/LMPC_node_profiler.jl +++ b/workspace/src/barc/src/LMPC_node_profiler.jl @@ -18,21 +18,6 @@ include("helper/solveMpcProblem.jl") include("helper/computeCostLap.jl") include("helper/functions.jl") -# Load Variables and create Model: -println("Loading and defining variables...") -include("helper/createModel.jl") - -# Initialize model by solving it once -println("Initial solve...") -solve(mdl) -println("Finished initial solve.") - -# Global variables: -z_est = zeros(1,4) -coeffCurvature_update = zeros(5) # use extra update variables so that they're not suddenly changed within functions -s_start_update = 0.0 - - function main() println("now starting the node") # initiate node, set up publisher / subscriber topics @@ -45,32 +30,29 @@ function main() save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps # Define and initialize variables - global mpcParams, trackCoeff, modelParams, z_est, coeffCurvature_update, s_start_update oldTraj = OldTrajectory() posInfo = PosInfo() mpcCoeff = MpcCoeff() lapStatus = LapStatus(1,1) mpcSol = MpcSol() - - posInfo.s_start = 0 - posInfo.s_target = 8.281192 - - mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs - mpcParams.R = 0.0*[1,1] # cost matrix for control inputs - mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v - - oldTraj.oldTraj = zeros(buffersize,4,2) - oldTraj.oldInput = zeros(buffersize,2,2) - - mpcCoeff.order = 5 - mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) - mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) # nz-1 because no coeff for s - mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon - - lapStatus.currentLap = 1 # initialize lap number - lapStatus.currentIt = 0 # current iteration in lap - + trackCoeff = TrackCoeff() # info about track (at current position, approximated) + modelParams = ModelParams() + mpcParams = MpcParams() + + z_est = zeros(4) + coeffCurvature_update = zeros(5) + s_start_update = [0.0] + #cmd::ECU + cmd = ECU(0.0,0.0) + #mdl::JuMP.Model + + #pub = Publisher("ecu", ECU, queue_size=10) + #pub2 = Publisher("logging", Logging, queue_size=10) + # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: + #s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) + + InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + println("Finished initialization.") # Lap parameters switchLap = false # initialize lap lap trigger s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] @@ -82,17 +64,30 @@ function main() zCurr_export = zeros(buffersize,4) uCurr_export = zeros(buffersize,2) - # Precompile functions by running them once: - solve(mdl) - #coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) - precompile(saveOldTraj,(OldTrajectory,Array{Float64},Array{Float64},LapStatus,Int64,Float64)) + # DEFINE MODEL *************************************************************************** + # **************************************************************************************** + println("Building model...") + + z_Init = zeros(4) + + mdl = MpcModel() + InitializeModel(mdl,mpcParams,modelParams,trackCoeff,z_Init) + + + # Initial solve: + println("Initial solve...") + solve(mdl.mdl) + solve(mdl.mdl) + println("Finished.") - z_sim = [0 0 0 0.5] - #while ! is_shutdown() - for j=1:300 + z_sim = zeros(4) + for j=1:100 z_sim[1] += 0.05 z_est = z_sim - if z_est[1] > 0 # check if data has been received (s > 0) + if z_est[1] > 0 # check if data has been received (s > 0) + println("Received data: z_est = $z_est") + println("curvature = $coeffCurvature_update") + println("s_start = $s_start_update") # ============================= Initialize iteration parameters ============================= lapStatus.currentIt += 1 # count iteration @@ -100,15 +95,15 @@ function main() i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = z_est # update state information posInfo.s = z_est[1] # update position info - posInfo.s_start = s_start_update + posInfo.s_start = s_start_update[1] trackCoeff.coeffCurvature = coeffCurvature_update # ======================================= Lap trigger ======================================= # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... - z_sim[1] = 0 # ... then select and save data + z_sim[1] = 0 println("Saving data") tic() saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,dt) @@ -153,15 +148,15 @@ function main() #println("Found coefficients: mpcCoeff = $mpcCoeff") # Solve the MPC problem tic() - solveMpcProblem(mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') tt = toq() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] #println("trackCoeff = $trackCoeff") println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") # ... and publish data - # cmd = ECU(mpcSol.a_x, mpcSol.d_f) - # publish(pub, cmd) + #cmd = ECU(mpcSol.a_x, mpcSol.d_f) + #publish(pub, cmd) zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) println("\n") diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index cb522590..2f931029 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(1.0,2.0,0.2,array([0.0,-1.0]),0) + l.create_racetrack(5.0,2.0,0.2,array([0.0,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index 1ddd31c9..05a652b7 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -35,8 +35,8 @@ type MpcParams # parameters for MPC solver Q::Array{Float64,1} R::Array{Float64,1} vPathFollowing::Float64 - QderivZ::Array{Float64} - QderivU::Array{Float64} + QderivZ::Array{Float64,1} + QderivU::Array{Float64,1} MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[]) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) end @@ -50,11 +50,11 @@ end type MpcSol # MPC solution output a_x::Float64 d_f::Float64 - solverStatus::Int64 + solverStatus::Symbol u::Array{Float64} z::Array{Float64} cost::Array{Float64} - MpcSol(a_x=0.0,d_f=0.0,solverStatus=1,u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) + MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) end type TrackCoeff # coefficients of track @@ -75,4 +75,37 @@ type ModelParams z_ub::Array{Float64} c0::Array{Float64} ModelParams(l_A=0.25,l_B=0.25,dt=0.1,u_lb=Float64[],u_ub=Float64[],z_lb=Float64[],z_ub=Float64[],c0=Float64[]) = new(l_A,l_B,dt,u_lb,u_ub,z_lb,z_ub,c0) -end \ No newline at end of file +end + +type MpcModel + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + ParInt::JuMP.Variable + + dsdt::Array{JuMP.NonlinearExpression,1} + bta::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearExpression,1} + + MpcModel(mdl=JuMP.Model(), + z0=@NLparameter(mdl,z0[i=1:4]==0), + coeff=@NLparameter(mdl,coeff[i=1:5]==0), + z_Ol=@variable(mdl,[1:4,1:10]), + u_Ol=@variable(mdl,[1:2,1:9]), + ParInt=@variable(mdl,0<=ParInt<=1), + dsdt=@NLexpression(mdl,dsdt[1:10],0), + bta=@NLexpression(mdl,bta[1:10],0), + c=@NLexpression(mdl,c[1:10],0)) = new(mdl, + z0, + coeff, + z_Ol, + u_Ol, + ParInt, + dsdt, + bta, + c) +end diff --git a/workspace/src/barc/src/helper/createModel.jl b/workspace/src/barc/src/helper/createModel.jl deleted file mode 100644 index 11afc208..00000000 --- a/workspace/src/barc/src/helper/createModel.jl +++ /dev/null @@ -1,83 +0,0 @@ -# Define Variables -trackCoeff = TrackCoeff() # info about track (at current position, approximated) -modelParams = ModelParams() -mpcParams = MpcParams() - -# =============================== -# Initialize Parameters -# =============================== - - -mpcParams.N = 5 -mpcParams.nz = 4 -mpcParams.Q = [0.0,1.0,1.0,1.0] # put weights on ey, epsi and v -mpcParams.R = [0.0,0.0] # put weights on ey, epsi and v -mpcParams.vPathFollowing = 0.5 - -trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) -trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation -trackCoeff.width = 0.4 # width of the track (0.5m) - -modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering -modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds -modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states -modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds -modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) -modelParams.l_A = 0.25 -modelParams.l_B = 0.25 -modelParams.dt = 0.1 - -z_Init = zeros(4,1) - -# ================================= -# Create Model -# ================================= -println("Building model...") -dt = modelParams.dt -L_a = modelParams.l_A -L_b = modelParams.l_B -N = mpcParams.N -c0 = modelParams.c0 - -mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1))#,linear_solver="ma57",print_user_options="yes")) - -#@variable( mdl, modelParams.z_ub[i,j] >= z_Ol[i=1:4,j=1:(N+1)] >= modelParams.z_lb[i,j]) # z = s, ey, epsi, v -@variable( mdl, z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v -#@variable( mdl, modelParams.u_ub[i,j] >= u_Ol[i=1:2,j=1:N] >= modelParams.u_lb[i,j]) # u = a, d_f -@variable( mdl, u_Ol[1:2,1:N]) - -for i=1:2 # I don't know why but somehow the short method returns errors sometimes - for j=1:N - setlowerbound(u_Ol[i,j], modelParams.u_lb[i,j]) - setupperbound(u_Ol[i,j], modelParams.u_ub[i,j]) - end -end -#= -for i=1:4 - for j=1:N+1 - setlowerbound(z_Ol[i,j], modelParams.z_lb[i,j]) - setupperbound(z_Ol[i,j], modelParams.z_ub[i,j]) - end -end -=# - -#@constraint(mdl, [i=1:2,j=1:N], u_Ol[i,j] <= modelParams.u_ub[i,j]) -@variable( mdl, 1 >= ParInt >= 0 ) - -@NLparameter(mdl, z0[i=1:4] == z_Init[i]) -@NLconstraint(mdl, [i=1:4], z_Ol[i,1] == z0[i]) - -@NLparameter(mdl, coeff[i=1:length(trackCoeff.coeffCurvature)] == trackCoeff.coeffCurvature[i]); - -@NLexpression(mdl, c[i = 1:N], coeff[1]*z_Ol[1,i]^4+coeff[2]*z_Ol[1,i]^3+coeff[3]*z_Ol[1,i]^2+coeff[4]*z_Ol[1,i]+coeff[5]) -#@NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[1,i]^(5-j),j=1:5}) # gives segmentation fault (probably doesn't like x^0 = 1) -@NLexpression(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( c0[3]*u_Ol[2,i] + c0[4]*abs(u_Ol[2,i])*u_Ol[2,i]) ) ) -@NLexpression(mdl, dsdt[i = 1:N], z_Ol[4,i]*cos(z_Ol[3,i]+bta[i])/(1-z_Ol[2,i]*c[i])) - -# System dynamics -for i=1:N - @NLconstraint(mdl, z_Ol[1,i+1] == z_Ol[1,i] + dt*dsdt[i] ) # s - @NLconstraint(mdl, z_Ol[2,i+1] == z_Ol[2,i] + dt*z_Ol[4,i]*sin(z_Ol[3,i]+bta[i]) ) # ey - @NLconstraint(mdl, z_Ol[3,i+1] == z_Ol[3,i] + dt*(z_Ol[4,i]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) # epsi - @NLconstraint(mdl, z_Ol[4,i+1] == z_Ol[4,i] + dt*(c0[1]*u_Ol[1,i] - c0[2]*abs(z_Ol[4,i]) * z_Ol[4,i])) # v -end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 54607528..a2f6b6ad 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -28,4 +28,89 @@ function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{F #save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] +end + +function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, + posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) + mpcParams.N = 10 + mpcParams.nz = 4 + mpcParams.Q = [0.0,10.0,10.0,1.0] # put weights on ey, epsi and v + mpcParams.R = [0.0,0.0] # put weights on ey, epsi and v + mpcParams.vPathFollowing = 0.5 + mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs + mpcParams.vPathFollowing = 0.5 + + trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) + trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation + trackCoeff.width = 1.0 # width of the track (0.5m) + + modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering + modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds + modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states + modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds + #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) + modelParams.c0 = [1, 0.63, 1, 0] # BARC-specific parameters (measured) + modelParams.l_A = 0.125 + modelParams.l_B = 0.125 + modelParams.dt = 0.1 + + posInfo.s_start = 0 + posInfo.s_target = 16.281192 + + oldTraj.oldTraj = zeros(buffersize,4,2) + oldTraj.oldInput = zeros(buffersize,2,2) + + mpcCoeff.order = 5 + mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) + mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) # nz-1 because no coeff for s + mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon + + lapStatus.currentLap = 1 # initialize lap number + lapStatus.currentIt = 0 # current iteration in lap +end + +function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelParams,trackCoeff::TrackCoeff,z_Init::Array{Float64,1}) + + dt = modelParams.dt + L_a = modelParams.l_A + L_b = modelParams.l_B + c0 = modelParams.c0 + u_lb = modelParams.u_lb + u_ub = modelParams.u_ub + z_lb = modelParams.z_lb + z_ub = modelParams.z_ub + + N = mpcParams.N + + m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1))#,linear_solver="ma57",print_user_options="yes")) + + @variable( m.mdl, m.z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v + @variable( m.mdl, m.u_Ol[1:2,1:N]) + + for i=1:2 # I don't know why but somehow the short method returns errors sometimes + for j=1:N + setlowerbound(m.u_Ol[i,j], modelParams.u_lb[i,j]) + setupperbound(m.u_Ol[i,j], modelParams.u_ub[i,j]) + end + end + #@variable( m.mdl, 1 >= m.ParInt >= 0 ) + + @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) + @NLconstraint(m.mdl, [i=1:4], m.z_Ol[i,1] == m.z0[i]) + + @NLparameter(m.mdl, m.coeff[i=1:length(trackCoeff.coeffCurvature)] == trackCoeff.coeffCurvature[i]); + + @NLexpression(m.mdl, m.c[i = 1:N], m.coeff[1]*m.z_Ol[1,i]^4+m.coeff[2]*m.z_Ol[1,i]^3+m.coeff[3]*m.z_Ol[1,i]^2+m.coeff[4]*m.z_Ol[1,i]+m.coeff[5]) + @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( m.u_Ol[2,i] ) ) ) + @NLexpression(m.mdl, m.dsdt[i = 1:N], m.z_Ol[4,i]*cos(m.z_Ol[3,i]+m.bta[i])/(1-m.z_Ol[2,i]*m.c[i])) + + # System dynamics + for i=1:N + @NLconstraint(m.mdl, m.z_Ol[1,i+1] == m.z_Ol[1,i] + dt*m.dsdt[i] ) # s + @NLconstraint(m.mdl, m.z_Ol[2,i+1] == m.z_Ol[2,i] + dt*m.z_Ol[4,i]*sin(m.z_Ol[3,i]+m.bta[i]) ) # ey + @NLconstraint(m.mdl, m.z_Ol[3,i+1] == m.z_Ol[3,i] + dt*(m.z_Ol[4,i]/L_a*sin(m.bta[i])-m.dsdt[i]*m.c[i]) ) # epsi + @NLconstraint(m.mdl, m.z_Ol[4,i+1] == m.z_Ol[4,i] + dt*(c0[1]*m.u_Ol[1,i] - c0[2]*abs(m.z_Ol[4,i]) * m.z_Ol[4,i])) # v + end + end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 53a9f71e..1213ea40 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -1,5 +1,5 @@ # Variable definitions -# z_Ol[i,j] = z_OpenLoop, open loop prediction of the state, i = state, j = step +# mdl.z_Ol[i,j] = z_OpenLoop, open loop prediction of the state, i = state, j = step # States: # i = 1 -> s @@ -7,10 +7,9 @@ # i = 3 -> epsi # i = 4 -> v -function solveMpcProblem(mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uCurr::Array{Float64}) +function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uCurr::Array{Float64}) # Load Parameters - coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} N = mpcParams.N Q = mpcParams.Q @@ -29,8 +28,6 @@ function solveMpcProblem(mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams, sol_u::Array{Float64,2} - global mdl - println("************************************** MPC SOLVER **************************************") println("zCurr = $(zCurr')") println("s_start = $s_start") @@ -43,39 +40,39 @@ function solveMpcProblem(mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams, u_Ref = zeros(N,2) # Update current initial condition - setvalue(z0,zCurr) + setvalue(mdl.z0,zCurr) # Update curvature - setvalue(coeff,coeffCurvature) + setvalue(mdl.coeff,coeffCurvature) - @NLexpression(mdl, costZ, 0) - @NLexpression(mdl, costZTerm, 0) - @NLexpression(mdl, constZTerm, 0) - @NLexpression(mdl, derivCost, 0) - @NLexpression(mdl, laneCost, 0) + @NLexpression(mdl.mdl, costZ, 0) + @NLexpression(mdl.mdl, costZTerm, 0) + @NLexpression(mdl.mdl, constZTerm, 0) + @NLexpression(mdl.mdl, derivCost, 0) + @NLexpression(mdl.mdl, laneCost, 0) # Derivative cost # --------------------------------- - @NLexpression(mdl, derivCost, 0*sum{QderivZ[j]*((zCurr[j]-z_Ol[j,1])^2+sum{(z_Ol[j,i]-z_Ol[j,i+1])^2,i=1:N}),j=1:4} + - sum{QderivU[j]*((uCurr[j]-u_Ol[j,1])^2+sum{(u_Ol[j,i]-u_Ol[j,i+1])^2,i=1:N-1}),j=1:2}) + @NLexpression(mdl.mdl, derivCost, 0*sum{QderivZ[j]*((zCurr[j]-mdl.z_Ol[j,1])^2+sum{(mdl.z_Ol[j,i]-mdl.z_Ol[j,i+1])^2,i=1:N}),j=1:4} + + sum{QderivU[j]*((uCurr[j]-mdl.u_Ol[j,1])^2+sum{(mdl.u_Ol[j,i]-mdl.u_Ol[j,i+1])^2,i=1:N-1}),j=1:2}) # Lane cost # --------------------------------- - @NLexpression(mdl, laneCost, 0*sum{(0.5+0.5*tanh(50*(z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(z_Ol[2,i]+ey_max))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 0*sum{(0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max))),i=1:N+1}) # Control Input cost # --------------------------------- - @NLexpression(mdl, controlCost, 0.5*sum{R[j]*sum{(u_Ol[j,i]-u_Ref[i,j])^2,i=1:N},j=1:2}) + @NLexpression(mdl.mdl, controlCost, 0.5*sum{R[j]*sum{(mdl.u_Ol[j,i]-u_Ref[i,j])^2,i=1:N},j=1:2}) # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap # termStateErr = (ParInt*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + # + (1 - ParInt)*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl, constZTerm, 10*(sum{(ParInt*sum{coeffTermConst[i,1,j]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermConst[i,2,j]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3})) + @NLexpression(mdl.mdl, constZTerm, 10*(sum{(ParInt*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}-z_Ol[j+1,N+1])^2,j=1:3}) + @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) end # Terminal cost @@ -83,39 +80,57 @@ function solveMpcProblem(mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams, if lapStatus.currentLap > 2 # if at least in the 3rd lap # costZTerm = ParInt*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + # + (1-ParInt)*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl, costZTerm, ParInt*sum{coeffTermCost[i,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermCost[i,2]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) - #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) + @NLexpression(mdl.mdl, costZTerm, ParInt*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-ParInt)*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + #@NLexpression(mdl.mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) # line above not necessary since the polynomial goes to zero anyways! elseif lapStatus.currentLap == 2 # if we're in the second second lap # costZTerm = polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl, costZTerm, sum{coeffTermCost[i,1]*z_Ol[1,N+1]^(order+1-i),i=1:order+1}) - #@NLexpression(mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) + @NLexpression(mdl.mdl, costZTerm, sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + #@NLexpression(mdl.mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) end # State cost # --------------------------------- if lapStatus.currentLap == 1 # if we're in the first lap, just do path following - @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[i,j]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory + @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[i,j]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) - #@NLexpression(mdl, costZ_h, 0) # zero state cost after crossing the finish line - #@NLexpression(mdl, costZ, 1 + (costZ_h-1) * (0.5+0.5*tanh(50*(z_Ol[1,N+1]+s_start-s_target)))) - @NLexpression(mdl, costZ, 1) - #@NLexpression(mdl, costZ, 1) + #@NLexpression(mdl.mdl, costZ_h, 0) # zero state cost after crossing the finish line + #@NLexpression(mdl.mdl, costZ, 1 + (costZ_h-1) * (0.5+0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) + @NLexpression(mdl.mdl, costZ, 1) + #@NLexpression(mdl.mdl, costZ, 1) end - @NLobjective(mdl, Min, costZ + costZTerm + constZTerm + derivCost + controlCost + laneCost) + @NLobjective(mdl.mdl, Min, costZ + costZTerm + constZTerm + derivCost + controlCost + laneCost) + println("Model formulation:") + #println(mdl.mdl) # Solve Problem and return solution - sol_status = solve(mdl) - sol_u = getvalue(u_Ol) - #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(u_Ol),getvalue(z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + println("coeff: $(getvalue(mdl.coeff))") + println("z0: $(getvalue(mdl.z0))") + println("Solution status: $sol_status") + println("Objective value: $(getobjectivevalue(mdl.mdl))") + println("Control Cost: $(getvalue(controlCost))") + println("CostZ: $(getvalue(costZ))") + println("DerivCost: $(getvalue(derivCost))") + + println("cost_ey: $(0.5*sum(sol_z[2,:].^2)*Q[2])") + println("cost_ePsi: $(0.5*sum(sol_z[3,:].^2)*Q[3])") + println("cost_V: $(0.5*sum((sol_z[4,:]-z_Ref[:,4]').^2)*Q[4])") + #println("z:") + #println(getvalue(mdl.z_Ol)) + #println("u:") + #println(getvalue(mdl.u_Ol)) + #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(mdl.u_Ol),getvalue(mdl.z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) mpcSol.a_x = sol_u[1,1] mpcSol.d_f = sol_u[2,1] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) - #println(getvalue(z_Ol[1,N+1])) + #println(getvalue(mdl.z_Ol[1,N+1])) #println(getvalue(constZTerm)) #println("ParInt = $(getvalue(ParInt))") #println("u = $(sol_u[:,1])") @@ -124,13 +139,13 @@ function solveMpcProblem(mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams, # ss = collect(zCurr[1]:.01:zCurr[1]+0.3) # #p = [ss.^4 ss.^3 ss.^2 ss.^1 ss.^0] *coeffTermCost[:,:,1] # p = [ss.^4 ss.^3 ss.^2 ss.^1 ss.^0] *coeffTermConst[:,:,1,1] - # plot(ss,p,getvalue(z_Ol[1,N+1]),getvalue(costZTerm),"o") + # plot(ss,p,getvalue(mdl.z_Ol[1,N+1]),getvalue(costZTerm),"o") # grid() # readline() # end - # println(getvalue(z_Ol)) + # println(getvalue(mdl.z_Ol)) # println("==============") - # println(getvalue(u_Ol)) + # println(getvalue(mdl.u_Ol)) nothing end From de561fd8d87c1bf8a43c3aa00713b9af63ed6843 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 19 Sep 2016 09:00:30 -0700 Subject: [PATCH 022/183] LMPC simulation works with multiple laps. Still oscillating a bit around the track. --- eval_sim/eval_sim.jl | 5 ++ workspace/src/barc/src/LMPC_node.jl | 2 +- workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 15 ++++-- workspace/src/barc/src/helper/classes.jl | 4 +- workspace/src/barc/src/helper/functions.jl | 16 +++--- .../src/barc/src/helper/solveMpcProblem.jl | 50 +++++++++++-------- 7 files changed, 55 insertions(+), 39 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index d52f5083..0e837b2d 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -20,6 +20,7 @@ function eval_sim() imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] z = d["z"] + cmd_log = d["cmd_log"] plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") grid(1) @@ -32,6 +33,10 @@ function eval_sim() plot(z.t,z.z[:,4]) grid() legend(["Velocity"]) + figure() + plot(cmd_log.t,cmd_log.z) + legend(["a","d_f"]) + grid() end function eval_LMPC() diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 47a29152..cde427c9 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -34,7 +34,7 @@ function main() buffersize = 700 # Create data to be saved - save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps + save_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps # Define and initialize variables oldTraj = OldTrajectory() diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 2f931029..34fb7c94 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,7 +40,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(5.0,2.0,0.2,array([0.0,-1.0]),0) + l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.063) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 41304a13..b5f2f39e 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -46,6 +46,7 @@ buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) imu_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,1)) est_meas = Measurements{Float32}(0,zeros(Float32,buffersize,1),zeros(Float32,buffersize,4)) +cmd_log = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,4)) function simModel(z,u,dt,l_A,l_B) @@ -57,21 +58,24 @@ function simModel(z,u,dt,l_A,l_B) bta = atan(l_A/(l_A+l_B)*tan(u[2])) zNext = z - zNext[1] = z[1] + dt*(z[4]*cos(z[3]+bta)) # x + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v # Add process noise (depending on velocity) - zNext = zNext + 0.01*diagm([0.01*z[4],0.01*z[4],0.001,0.01*z[4]])*randn(4,1) + zNext = zNext + 0.0*diagm([0.01*z[4],0.01*z[4],0.001,0.01*z[4]])*randn(4,1) return zNext end function ECU_callback(msg::ECU) - global u_current - u_current = [msg.motor, msg.servo] + global u_current, t0 + u_current = [msg.motor, msg.servo] + cmd_log.i += 1 + cmd_log.t[cmd_log.i] = time()-t0 + cmd_log.z[cmd_log.i,:] = u_current' end function est_callback(msg::Z_KinBkMdl) @@ -176,13 +180,14 @@ function main() clean_up(gps_meas) clean_up(est_meas) clean_up(imu_meas) + clean_up(cmd_log) z_real.z[1:i-1,:] = z_current[1:i-1,:] z_real.i = i clean_up(z_real) # Save simulation data to file log_path = "$(homedir())/simulations/output.jld" - save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"imu_meas",imu_meas) + save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"imu_meas",imu_meas,"cmd_log",cmd_log) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") #writedlm(log_path,z_current[1:i-1,:]) end diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index 05a652b7..c6a6d8e5 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -85,7 +85,7 @@ type MpcModel z_Ol::Array{JuMP.Variable,2} u_Ol::Array{JuMP.Variable,2} - ParInt::JuMP.Variable + ParInt::Array{JuMP.Variable,1} dsdt::Array{JuMP.NonlinearExpression,1} bta::Array{JuMP.NonlinearExpression,1} @@ -96,7 +96,7 @@ type MpcModel coeff=@NLparameter(mdl,coeff[i=1:5]==0), z_Ol=@variable(mdl,[1:4,1:10]), u_Ol=@variable(mdl,[1:2,1:9]), - ParInt=@variable(mdl,0<=ParInt<=1), + ParInt=@variable(mdl,[1:1]), dsdt=@NLexpression(mdl,dsdt[1:10],0), bta=@NLexpression(mdl,bta[1:10],0), c=@NLexpression(mdl,c[1:10],0)) = new(mdl, diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index a2f6b6ad..3149d6ea 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -34,19 +34,18 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) mpcParams.N = 10 mpcParams.nz = 4 - mpcParams.Q = [0.0,10.0,10.0,1.0] # put weights on ey, epsi and v - mpcParams.R = [0.0,0.0] # put weights on ey, epsi and v - mpcParams.vPathFollowing = 0.5 + mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v + mpcParams.R = [0.0,0.0] # put weights on a and d_f mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 0.5 + mpcParams.QderivU = 1*[1,10] # cost matrix for derivative cost of inputs + mpcParams.vPathFollowing = 0.8 trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation - trackCoeff.width = 1.0 # width of the track (0.5m) + trackCoeff.width = 0.4 # width of the track (0.5m) modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering - modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds + modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) @@ -56,7 +55,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP modelParams.dt = 0.1 posInfo.s_start = 0 - posInfo.s_target = 16.281192 + posInfo.s_target = 10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) @@ -87,6 +86,7 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara @variable( m.mdl, m.z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v @variable( m.mdl, m.u_Ol[1:2,1:N]) + @variable( m.mdl, 0 <= m.ParInt[1:1] <= 1) for i=1:2 # I don't know why but somehow the short method returns errors sometimes for j=1:N diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 1213ea40..9b7ba378 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -57,7 +57,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Lane cost # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 0*sum{(0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 10*sum{(0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max))),i=1:N+1}) # Control Input cost # --------------------------------- @@ -66,10 +66,10 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - # termStateErr = (ParInt*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + - # + (1 - ParInt)*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl.mdl, constZTerm, 10*(sum{(ParInt*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) + # termStateErr = (mdl.ParInt[1]*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + + # + (1 - mdl.ParInt[1])*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; + @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) @@ -78,10 +78,10 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Terminal cost # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - # costZTerm = ParInt*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + - # + (1-ParInt)*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); - @NLexpression(mdl.mdl, costZTerm, ParInt*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-ParInt)*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + # costZTerm = mdl.ParInt[1]*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + + # + (1-mdl.ParInt[1])*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); + @NLexpression(mdl.mdl, costZTerm, mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) #@NLexpression(mdl.mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) # line above not necessary since the polynomial goes to zero anyways! elseif lapStatus.currentLap == 2 # if we're in the second second lap @@ -104,23 +104,29 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara @NLobjective(mdl.mdl, Min, costZ + costZTerm + constZTerm + derivCost + controlCost + laneCost) - println("Model formulation:") + #println("Model formulation:") #println(mdl.mdl) # Solve Problem and return solution sol_status = solve(mdl.mdl) sol_u = getvalue(mdl.u_Ol) sol_z = getvalue(mdl.z_Ol) - println("coeff: $(getvalue(mdl.coeff))") - println("z0: $(getvalue(mdl.z0))") - println("Solution status: $sol_status") - println("Objective value: $(getobjectivevalue(mdl.mdl))") - println("Control Cost: $(getvalue(controlCost))") - println("CostZ: $(getvalue(costZ))") - println("DerivCost: $(getvalue(derivCost))") - - println("cost_ey: $(0.5*sum(sol_z[2,:].^2)*Q[2])") - println("cost_ePsi: $(0.5*sum(sol_z[3,:].^2)*Q[3])") - println("cost_V: $(0.5*sum((sol_z[4,:]-z_Ref[:,4]').^2)*Q[4])") + + # COST PRINTS: ******************************************************** + # println("coeff: $(getvalue(mdl.coeff))") + # println("z0: $(getvalue(mdl.z0))") + # println("Solution status: $sol_status") + # println("Objective value: $(getobjectivevalue(mdl.mdl))") + # println("Control Cost: $(getvalue(controlCost))") + # println("CostZ: $(getvalue(costZ))") + # println("DerivCost: $(getvalue(derivCost))") + # println("LaneCost: $(getvalue(laneCost))") + # println("costZTerm: $(getvalue(costZTerm))") + # println("constZTerm: $(getvalue(constZTerm))") + + # println("cost_ey: $(0.5*sum(sol_z[2,:].^2)*Q[2])") + # println("cost_ePsi: $(0.5*sum(sol_z[3,:].^2)*Q[3])") + # println("cost_V: $(0.5*sum((sol_z[4,:]-z_Ref[:,4]').^2)*Q[4])") + #println("z:") #println(getvalue(mdl.z_Ol)) #println("u:") @@ -132,7 +138,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara #println(getvalue(costZTerm)) #println(getvalue(mdl.z_Ol[1,N+1])) #println(getvalue(constZTerm)) - #println("ParInt = $(getvalue(ParInt))") + #println("mdl.ParInt[1] = $(getvalue(mdl.ParInt[1]))") #println("u = $(sol_u[:,1])") # if lapStatus.currentLap > 100 From 3b8bff8b9ee0e542b4dcb66f6337ccefe84834e0 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 19 Sep 2016 17:13:15 -0700 Subject: [PATCH 023/183] Added evaluation functions, changed tabs to spaces (in python code), generalized code for polynomial of coefficients. --- eval_sim/eval_sim.jl | 49 +- workspace/src/barc/src/LMPC_node.jl | 40 +- .../src/barc/src/Localization_helpers.py | 458 +++++++++--------- .../barc/src/helper/coeffConstraintCost.jl | 17 - workspace/src/barc/src/helper/functions.jl | 11 +- .../src/barc/src/helper/solveMpcProblem.jl | 14 +- 6 files changed, 329 insertions(+), 260 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 0e837b2d..bcedb67d 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -9,9 +9,9 @@ type Measurements{T} end -log_path = "$(homedir())/simulations/output.jld" -log_path_LMPC = "$(homedir())/simulations/LMPC_output.jld" -log_path_profile = "$(homedir())/simulations/profile.jlprof" +const log_path = "$(homedir())/simulations/output.jld" +const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" +const log_path_profile = "$(homedir())/simulations/profile.jlprof" function eval_sim() d = load(log_path) @@ -42,10 +42,53 @@ end function eval_LMPC() d = load(log_path_LMPC) oldTraj = d["oldTraj"] + t = d["t"] + state = d["state"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + cost = d["cost"] + curv = d["curv"] plot(oldTraj[:,:,1,1]) grid(1) + figure() + plot(t,state) + grid(1) + plot(t,cost) + grid(1) + legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) + figure() + plot(t,curv) + legend(["1","2","3","4","5","6","7"]) end +function anim_MPC(z) + figure() + hold(0) + grid(1) + for i=1:size(z,3) + plot(z[:,:,i]) + xlim([1,11]) + ylim([-2,2]) + sleep(0.1) + end +end + +function anim_curv(curv) + s = 0.0:.05:2.0 + figure() + hold(0) + ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + for i=1:size(curv,1) + c = ss*curv[i,:]' + plot(s,c) + xlim([0,2]) + ylim([-1.5,1.5]) + sleep(0.25) + end +end + + + function eval_prof() Profile.clear() @load "$(homedir())/simulations/profile.jlprof" diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index cde427c9..3f9c3793 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -27,14 +27,16 @@ end function main() println("now starting the node") - # initiate node, set up publisher / subscriber topics - init_node("mpc_traj") - loop_rate = Rate(10) - buffersize = 700 + buffersize = 700 # size of oldTraj buffers # Create data to be saved - save_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps + log_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps + log_t = zeros(10000,1) + log_state = zeros(10000,4) + log_sol_z = zeros(11,4,10000) + log_sol_u = zeros(10,2,10000) + log_cost = zeros(10000,6) # Define and initialize variables oldTraj = OldTrajectory() @@ -47,16 +49,21 @@ function main() mpcParams = MpcParams() z_est = zeros(4) - coeffCurvature_update = zeros(5) s_start_update = [0.0] cmd = ECU(0.0,0.0) + InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + + coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) + log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) + # Initialize ROS node and topics + init_node("mpc_traj") + loop_rate = Rate(10) pub = Publisher("ecu", ECU, queue_size=10) pub2 = Publisher("logging", Logging, queue_size=10) # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) - InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) println("Finished initialization.") # Lap parameters switchLap = false # initialize lap lap trigger @@ -84,6 +91,8 @@ function main() solve(mdl.mdl) println("Finished.") + t0 = time() + k = 0 while ! is_shutdown() if z_est[1] > 0 # check if data has been received (s > 0) println("Received data: z_est = $z_est") @@ -107,7 +116,7 @@ function main() println("Saving data") tic() saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) - save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + log_oldTraj[1:i,:,:,lapStatus.currentLap] = oldTraj.oldTraj[1:i,:,:] zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state uCurr[1,:] = uCurr[i+1,:] # ... and input i = 1 @@ -145,7 +154,7 @@ function main() end tt = toq() println("Finished coefficients, t = $tt s") - #println("Found coefficients: mpcCoeff = $mpcCoeff") + # Solve the MPC problem tic() solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') @@ -160,7 +169,16 @@ function main() zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) println("\n") - + + # Logging + k = k + 1 # counter + log_state[k,:] = z_est + log_t[k] = time() - t0 + log_sol_z[:,:,k] = mpcSol.z' + log_sol_u[:,:,k] = mpcSol.u' + log_cost[k,:] = mpcSol.cost + log_curv[k,:] = trackCoeff.coeffCurvature + else println("No estimation data received!") end @@ -169,7 +187,7 @@ function main() # Save simulation data to file log_path = "$(homedir())/simulations/output_LMPC.jld" - save(log_path,"oldTraj",save_oldTraj) + save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k],"cost",log_cost[1:k,:],"curv",log_curv[1:k,:]) println("Exiting LMPC node. Saved data.") end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index cf9e4f3f..365bbb07 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -2,225 +2,249 @@ import scipy.optimize def create_circle(rad,n,c): - ang = linspace(0,2*pi,n) - x = rad*cos(ang)+c[0] - y = rad*sin(ang)+c[1] - return array([x,y]) + ang = linspace(0,2*pi,n) + x = rad*cos(ang)+c[0] + y = rad*sin(ang)+c[1] + return array([x,y]) class Localization: - n = 0 # number of nodes - c = 0 # center of circle (in case of circular trajectory) - pos = 0 # current position - psi = 0 # current orientation - nodes = 0 # all nodes are saved in a matrix - N_nodes_poly_back = 10 # number of nodes behind current position - N_nodes_poly_front = 30 # number of nodes in front - ds = 0 # distance between nodes - nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 6 # order of x-y-polynomial interpolation - OrderThetaCurv = 4 # order of theta interpolation - closed = True # open or closed trajectory? - - coeffX = 0 - coeffY = 0 - coeffTheta = 0 - coeffCurvature = 0 - - s = 0 # distance from s_start to current closest node (idx_min) - s_start = 0 # distance along path from first node to start node (which is N_nodes_poly_back behind current closest node) - ey = 0 # lateral distance to path - epsi = 0 # error in psi (between current psi and trajectory tangent) - v = 0 # current velocity (not necessary for these calculations but for MPC) - - def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to make sure that all points are equidistant! - ang = linspace(0,2*pi-2*pi/n,n) - #print(ang) - x = rad*cos(ang)+c[0] - y = rad*sin(ang)+c[1] - self.nodes = array([x,y]) - self.n = n - self.c = c - self.rad = rad - #self.ds = rad*2*pi/n - self.ds = 2*rad*tan(2*pi/n/2) - def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points - x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines - s = size(x) - da = 2*pi/360*5 - x = hstack((x,L/2.0+b/2*cos(arange(pi/2,-pi/2,-da)))) - x = hstack((x,linspace(L/2.0,-L/2.0,5))) - x = hstack((x,-L/2.0+b/2*cos(arange(pi/2,1.5*pi+da,da)))) - x = hstack((x,linspace(-L/2.0,0,5))) - - y = ones(s)*b/2.0 - y = hstack((y,b/2*sin(arange(pi/2,-pi/2,-da)))) - y = hstack((y,-ones(s)*b/2)) - y = hstack((y,-b/2*sin(arange(pi/2,1.5*pi+da,da)))) - y = hstack((y,ones(s)*b/2.0)) - - s_tot = size(x) - - R = array([[cos(ang),-sin(ang)],[sin(ang),cos(ang)]]) # Rotation Matrix - - nodes = dot(R,array([x,y]))+(c*ones([2,s_tot]).T).T - - self.nodes = nodes - self.n = size(x) - self.ds = ds - - def create_ellipse(self,L_a,L_b,n,c=array([0,0])): - ang = linspace(0,2*pi,n) - x = L_a*cos(ang)+c[0] - y = L_b*sin(ang)+c[1] - self.nodes = array([x,y]) - self.n = n - self.c = c - - def prepare_trajectory(self,ds_in): # brings trajectory to correct format (equidistant nodes with distances around ds) - x = self.nodes[0] - y = self.nodes[1] - n = size(x) # number of nodes - ds = (diff(x)**2+diff(y)**2)**0.5 # distances between nodes - #if self.closed: - # ds = hstack((ds,((x[n-1]-x[0])**2+(y[n-1]-y[0])**2)**0.5)) - s = hstack((0,cumsum(ds))) - length = sum(ds) - print "Track length = %fm"%length - dsn = ds_in # optimal new step size (might be calculated externally for now) - rem = lambda x:mod(length,x) # function to calculate optimal step size (for evenly distributed steps) - sol = scipy.optimize.fmin(func=rem,x0=dsn) - dsn = sol[0] - sn = arange(0,length,dsn) # new steps - xn = interp(sn,s,x) - yn = interp(sn,s,y) - - self.nodes = array([xn,yn]) - self.ds = dsn - self.n = size(xn) - - - def set_pos(self,x,y,psi,v): - self.pos = array([x,y]) - self.psi = psi - self.v = v - - def find_s(self): - dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of pos to all nodes - #mindist = amin(dist) # minimum distance - idx_min = argmin(dist) # index of minimum distance - n = self.n - nPoints = self.nPoints - # Use closest node to determine start and end of polynomial approximation - idx_start = idx_min - self.N_nodes_poly_back - idx_end = idx_min + self.N_nodes_poly_front - if self.closed == True: - if idx_start<0: - nodes_X = hstack((self.nodes[0,n+idx_start:n],self.nodes[0,0:idx_end+1])) - nodes_Y = hstack((self.nodes[1,n+idx_start:n],self.nodes[1,0:idx_end+1])) - idx_start = n+idx_start - elif idx_end>n-1: - nodes_X = hstack((self.nodes[0,idx_start:n],self.nodes[0,0:idx_end+1-n])) - nodes_Y = hstack((self.nodes[1,idx_start:n],self.nodes[1,0:idx_end+1-n])) - else: - nodes_X = self.nodes[0,idx_start:idx_end+1] - nodes_Y = self.nodes[1,idx_start:idx_end+1] - else: - if idx_start<0: - nodes_X = self.nodes[0,0:nPoints] - nodes_Y = self.nodes[1,0:nPoints] - idx_start = 0 - elif idx_end>n-1: - nodes_X = self.nodes[0,n-nPoints:n] - nodes_Y = self.nodes[1,n-nPoints:n] - idx_start = n-nPoints - else: - nodes_X = self.nodes[0,idx_start:idx_end+1] - nodes_Y = self.nodes[1,idx_start:idx_end+1] - - # Create Matrix for interpolation - # x-y-Matrix - Matrix = zeros([self.nPoints,self.OrderXY+1]) - for i in range(0,self.nPoints): - for k in range(0,self.OrderXY+1): - Matrix[i,self.OrderXY-k] = (i*self.ds)**k - - # curvature matrix - Matrix3rd = zeros([self.nPoints,self.OrderThetaCurv+1]) - for i in range(0,self.nPoints): - for k in range(0,self.OrderThetaCurv+1): - Matrix3rd[i,self.OrderThetaCurv-k] = (i*self.ds)**k - - # Solve system of equations to find polynomial coefficients (for x-y) - self.coeffX = linalg.lstsq(Matrix,nodes_X)[0] - self.coeffY = linalg.lstsq(Matrix,nodes_Y)[0] - - # find angles and curvature along interpolation polynomial - b_theta_vec = zeros(self.nPoints) - b_curvature_vector = zeros(self.nPoints) - - for j in range(0,self.nPoints): - s = j*self.ds - dX = polyval(polyder(self.coeffX,1),s) - dY = polyval(polyder(self.coeffY,1),s) - ddX = polyval(polyder(self.coeffX,2),s) - ddY = polyval(polyder(self.coeffY,2),s) - angle = arctan2(dY,dX) - if j>1: - if angle - b_theta_vec[j-1] > pi: - angle = angle - 2*pi - elif angle - b_theta_vec[j-1] < -pi: - angle = angle + 2*pi - - b_theta_vec[j] = angle - b_curvature_vector[j] = (dX*ddY-dY*ddX)/(dX**2+dY**2)**1.5 - - # calculate coefficients for Theta and curvature - coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] - coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] - - # Calculate s - discretization = 0.01 - s_idx_start = max(0,idx_min-1) - if self.closed and s_idx_start-idx_start<0: - s_idx_start = s_idx_start + n - j = arange((s_idx_start-idx_start)*self.ds,(s_idx_start-idx_start+2)*self.ds,discretization) - x_eval = polyval(self.coeffX,j) - y_eval = polyval(self.coeffY,j) - dist_eval = sum((self.pos*ones([size(j),2])-vstack((x_eval,y_eval)).transpose())**2,1)**0.5 - idx_s_min = argmin(dist_eval) - s = j[idx_s_min] # s = minimum distance to points between idx_min-1 and idx_min+1 - eyabs = amin(dist_eval) # absolute distance to curve - - # Calculate sign of y - s0 = s - discretization - XCurve0 = polyval(self.coeffX,s0) - YCurve0 = polyval(self.coeffY,s0) - XCurve = polyval(self.coeffX,s) - YCurve = polyval(self.coeffY,s) - dX = polyval(polyder(self.coeffX,1),s) - dY = polyval(polyder(self.coeffY,1),s) - - xyVectorAngle = arctan2(self.pos[1]-YCurve0, self.pos[0]-XCurve0) - xyPathAngle = arctan2(dY,dX) - ey = eyabs*sign(sin(xyVectorAngle-xyPathAngle)) - - # Calculate epsi - epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle - epsi = (epsi+pi)%(2*pi)-pi - #if abs(epsi) > pi/2: - # if epsi < pi/2: - # epsi = epsi + 2*pi - # else: - # epsi = epsi - 2*pi + n = 0 # number of nodes + c = 0 # center of circle (in case of circular trajectory) + pos = 0 # current position + psi = 0 # current orientation + nodes = 0 # all nodes are saved in a matrix + N_nodes_poly_back = 10 # number of nodes behind current position + N_nodes_poly_front = 40 # number of nodes in front + ds = 0 # distance between nodes + nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total + OrderXY = 6 # order of x-y-polynomial interpolation + OrderThetaCurv = 6 # order of theta interpolation + closed = True # open or closed trajectory? + + coeffX = 0 + coeffY = 0 + coeffTheta = 0 + coeffCurvature = 0 + + s = 0 # distance from s_start to current closest node (idx_min) + s_start = 0 # distance along path from first node to start node (which is N_nodes_poly_back behind current closest node) + ey = 0 # lateral distance to path + epsi = 0 # error in psi (between current psi and trajectory tangent) + v = 0 # current velocity (not necessary for these calculations but for MPC) + + def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to make sure that all points are equidistant! + ang = linspace(0,2*pi-2*pi/n,n) + #print(ang) + x = rad*cos(ang)+c[0] + y = rad*sin(ang)+c[1] + self.nodes = array([x,y]) + self.n = n + self.c = c + self.rad = rad + #self.ds = rad*2*pi/n + self.ds = 2*rad*tan(2*pi/n/2) + def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points + x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines + s = size(x) + da = 2*pi/360*5 + x = hstack((x,L/2.0+b/2*cos(arange(pi/2,-pi/2,-da)))) + x = hstack((x,linspace(L/2.0,-L/2.0,5))) + x = hstack((x,-L/2.0+b/2*cos(arange(pi/2,1.5*pi+da,da)))) + x = hstack((x,linspace(-L/2.0,0,5))) + + y = ones(s)*b/2.0 + y = hstack((y,b/2*sin(arange(pi/2,-pi/2,-da)))) + y = hstack((y,-ones(s)*b/2)) + y = hstack((y,-b/2*sin(arange(pi/2,1.5*pi+da,da)))) + y = hstack((y,ones(s)*b/2.0)) + + s_tot = size(x) + + R = array([[cos(ang),-sin(ang)],[sin(ang),cos(ang)]]) # Rotation Matrix + + nodes = dot(R,array([x,y]))+(c*ones([2,s_tot]).T).T + + self.nodes = nodes + self.n = size(x) + self.ds = ds + + def create_ellipse(self,L_a,L_b,n,c=array([0,0])): + ang = linspace(0,2*pi,n) + x = L_a*cos(ang)+c[0] + y = L_b*sin(ang)+c[1] + self.nodes = array([x,y]) + self.n = n + self.c = c + + def prepare_trajectory(self,ds_in): # brings trajectory to correct format (equidistant nodes with distances around ds) + x = self.nodes[0] + y = self.nodes[1] + n = size(x) # number of nodes + ds = (diff(x)**2+diff(y)**2)**0.5 # distances between nodes + #if self.closed: + # ds = hstack((ds,((x[n-1]-x[0])**2+(y[n-1]-y[0])**2)**0.5)) + s = hstack((0,cumsum(ds))) + length = sum(ds) + dsn = ds_in # optimal new step size (might be calculated externally for now) + rem = lambda x:mod(length,x) # function to calculate optimal step size (for evenly distributed steps) + sol = scipy.optimize.fmin(func=rem,x0=dsn) + dsn = sol[0] + #sn = arange(0,length,dsn) # new steps + #n = size(sn,0) + n = floor(length/dsn)+1 + #print(n) + sn = arange(0,n*dsn,dsn) + #print(sn) + xn = interp(sn,s,x) + yn = interp(sn,s,y) + + # if length - sn[-1] < dsn: + # sn = sn[0:n-1] + # xn = xn[0:n-1] + # yn = yn[0:n-1] + # n = n - 1 + + self.nodes = array([xn,yn]) + self.ds = dsn + self.n = size(xn) + print "Track length = %fm, ds = %fm"%(length,dsn) + print "Approximated length = %fm"%(self.nPoints*dsn) + #print(sn) + #print(self.nodes) + + + def set_pos(self,x,y,psi,v): + self.pos = array([x,y]) + self.psi = psi + self.v = v + + def find_s(self): + dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of current position to all nodes + idx_min = argmin(dist) # index of minimum distance + #print("closest point: %f"%idx_min) + n = self.n # number of nodes + nPoints = self.nPoints # number of points for polynomial approximation (around current position) + + # Use closest node to determine start and end of polynomial approximation + idx_start = idx_min - self.N_nodes_poly_back + idx_end = idx_min + self.N_nodes_poly_front + if self.closed == True: # if the track is modeled as closed (start = end) + if idx_start<0: # and if the start for polynomial approx. is before the start line + nodes_X = hstack((self.nodes[0,n+idx_start:n],self.nodes[0,0:idx_end+1])) # then stack the end and beginning of a lap together + nodes_Y = hstack((self.nodes[1,n+idx_start:n],self.nodes[1,0:idx_end+1])) + idx_start = n+idx_start + elif idx_end>n-1: # if the end is behind the finish line + nodes_X = hstack((self.nodes[0,idx_start:n],self.nodes[0,0:idx_end+1-n])) # then stack the end and beginning of the lap together + nodes_Y = hstack((self.nodes[1,idx_start:n],self.nodes[1,0:idx_end+1-n])) + else: # if we are somewhere in the middle of the track + nodes_X = self.nodes[0,idx_start:idx_end+1] # then just use the nodes from idx_start to end for interpolation + nodes_Y = self.nodes[1,idx_start:idx_end+1] + else: # if the track is not closed + if idx_start<0: + nodes_X = self.nodes[0,0:nPoints] + nodes_Y = self.nodes[1,0:nPoints] + idx_start = 0 + elif idx_end>n-1: + nodes_X = self.nodes[0,n-nPoints:n] + nodes_Y = self.nodes[1,n-nPoints:n] + idx_start = n-nPoints + else: + nodes_X = self.nodes[0,idx_start:idx_end+1] + nodes_Y = self.nodes[1,idx_start:idx_end+1] + + # Create Matrix for interpolation + # x-y-Matrix + Matrix = zeros([self.nPoints,self.OrderXY+1]) + for i in range(0,self.nPoints): + for k in range(0,self.OrderXY+1): + Matrix[i,self.OrderXY-k] = (i*self.ds)**k + + # curvature matrix + Matrix3rd = zeros([self.nPoints,self.OrderThetaCurv+1]) + for i in range(0,self.nPoints): + for k in range(0,self.OrderThetaCurv+1): + Matrix3rd[i,self.OrderThetaCurv-k] = (i*self.ds)**k + + # Solve system of equations to find polynomial coefficients (for x-y) + self.coeffX = linalg.lstsq(Matrix,nodes_X)[0] + self.coeffY = linalg.lstsq(Matrix,nodes_Y)[0] + + # find angles and curvature along interpolation polynomial + b_theta_vec = zeros(self.nPoints) + b_curvature_vector = zeros(self.nPoints) + + for j in range(0,self.nPoints): + s = j*self.ds + dX = polyval(polyder(self.coeffX,1),s) + dY = polyval(polyder(self.coeffY,1),s) + ddX = polyval(polyder(self.coeffX,2),s) + ddY = polyval(polyder(self.coeffY,2),s) + angle = arctan2(dY,dX) + if j>1: + if angle - b_theta_vec[j-1] > pi: + angle = angle - 2*pi + elif angle - b_theta_vec[j-1] < -pi: + angle = angle + 2*pi + + b_theta_vec[j] = angle + b_curvature_vector[j] = (dX*ddY-dY*ddX)/(dX**2+dY**2)**1.5 # this calculates the curvature values for all points in the interp. interval + # these values are going to be approximated by a polynomial! + + # calculate coefficients for Theta and curvature + coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] + coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] + + #if max(coeffCurvature) > 50: + # print "Large value, idx_min = %f"%(idx_min) + # print(nodes_X) + # print(nodes_Y) + + + # Calculate s + discretization = 0.01 # discretization to calculate s + s_idx_start = max(0,idx_min-1) # where the discretization starts + if self.closed and s_idx_start-idx_start<0: + s_idx_start = s_idx_start + n + j = arange((s_idx_start-idx_start)*self.ds,(s_idx_start-idx_start+2)*self.ds,discretization) + #print("s_discretization:") + #print(j) + x_eval = polyval(self.coeffX,j) + y_eval = polyval(self.coeffY,j) + dist_eval = sum((self.pos*ones([size(j),2])-vstack((x_eval,y_eval)).transpose())**2,1)**0.5 + idx_s_min = argmin(dist_eval) + s = j[idx_s_min] # s = minimum distance to points between idx_min-1 and idx_min+1 + eyabs = amin(dist_eval) # absolute distance to curve + + # Calculate sign of y + s0 = s - discretization + XCurve0 = polyval(self.coeffX,s0) + YCurve0 = polyval(self.coeffY,s0) + XCurve = polyval(self.coeffX,s) + YCurve = polyval(self.coeffY,s) + dX = polyval(polyder(self.coeffX,1),s) + dY = polyval(polyder(self.coeffY,1),s) + + xyVectorAngle = arctan2(self.pos[1]-YCurve0, self.pos[0]-XCurve0) + xyPathAngle = arctan2(dY,dX) + ey = eyabs*sign(sin(xyVectorAngle-xyPathAngle)) + + # Calculate epsi + epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle + epsi = (epsi+pi)%(2*pi)-pi + #if abs(epsi) > pi/2: + # if epsi < pi/2: + # epsi = epsi + 2*pi + # else: + # epsi = epsi - 2*pi #epsi = (self.psi+pi)%(2*pi)+pi-xyPathAngle - self.epsi = epsi - self.ey = ey - self.s = s - self.coeffTheta = coeffTheta - self.coeffCurvature = coeffCurvature - self.s_start = idx_start*self.ds + self.epsi = epsi + self.ey = ey + self.s = s + self.coeffTheta = coeffTheta + self.coeffCurvature = coeffCurvature + self.s_start = idx_start*self.ds - def __init__(self): - self.x = 0 - self.y = 0 + def __init__(self): + self.x = 0 + self.y = 0 diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index 8a47936a..9d3a630d 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -18,7 +18,6 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # coeffCost # Read Inputs - tic() lapNumber = lapStatus.currentLap s_start = posInfo.s_start @@ -61,11 +60,6 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # Compute the index DistS = ( s_total - oldS ).^2 - - tt=toq() - println("Loading parameters: $tt") - tic() - idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) @@ -78,9 +72,6 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo end # bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) - tt = toq() - println("2nd: $tt") - tic() println("************************************** COEFFICIENTS **************************************") println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") println("s_total = $s_total") @@ -103,18 +94,12 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo end # Compute the coefficients - println("Calculating coefficients...") coeffConst = zeros(Order+1,2,3) for i=1:2 coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldV[vec_range[i]] end - println("Done.") - - tt = toq() - println("3rd: $tt") - tic() # Finished with calculating the constraint coefficients @@ -149,8 +134,6 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector end - tt = toq() - println("4th: $tt") mpcCoeff.coeffCost = coeffCost mpcCoeff.coeffConst = coeffConst diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 3149d6ea..7110dc00 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -40,8 +40,8 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.QderivU = 1*[1,10] # cost matrix for derivative cost of inputs mpcParams.vPathFollowing = 0.8 - trackCoeff.coeffCurvature = [0.0,0.0,0.0,0.0,0.0] # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.nPolyCurvature = 4 # 4th order polynomial for curvature approximation + trackCoeff.nPolyCurvature = 6 # 4th order polynomial for curvature approximation + trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.width = 0.4 # width of the track (0.5m) modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering @@ -81,6 +81,8 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara z_ub = modelParams.z_ub N = mpcParams.N + + n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1))#,linear_solver="ma57",print_user_options="yes")) @@ -99,9 +101,10 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) @NLconstraint(m.mdl, [i=1:4], m.z_Ol[i,1] == m.z0[i]) - @NLparameter(m.mdl, m.coeff[i=1:length(trackCoeff.coeffCurvature)] == trackCoeff.coeffCurvature[i]); + @NLparameter(m.mdl, m.coeff[i=1:n_poly_curv+1] == trackCoeff.coeffCurvature[i]); - @NLexpression(m.mdl, m.c[i = 1:N], m.coeff[1]*m.z_Ol[1,i]^4+m.coeff[2]*m.z_Ol[1,i]^3+m.coeff[3]*m.z_Ol[1,i]^2+m.coeff[4]*m.z_Ol[1,i]+m.coeff[5]) + #@NLexpression(m.mdl, m.c[i = 1:N], m.coeff[1]*m.z_Ol[1,i]^4+m.coeff[2]*m.z_Ol[1,i]^3+m.coeff[3]*m.z_Ol[1,i]^2+m.coeff[4]*m.z_Ol[1,i]+m.coeff[5]) + @NLexpression(m.mdl, m.c[i = 1:N], sum{m.coeff[j]*m.z_Ol[1,i]^(n_poly_curv-j+1),j=1:n_poly_curv} + m.coeff[n_poly_curv+1]) @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( m.u_Ol[2,i] ) ) ) @NLexpression(m.mdl, m.dsdt[i = 1:N], m.z_Ol[4,i]*cos(m.z_Ol[3,i]+m.bta[i])/(1-m.z_Ol[2,i]*m.c[i])) diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 9b7ba378..04f45bfd 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -27,6 +27,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara v_ref = mpcParams.vPathFollowing sol_u::Array{Float64,2} + sol_z::Array{Float64,2} println("************************************** MPC SOLVER **************************************") println("zCurr = $(zCurr')") @@ -68,26 +69,20 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara if lapStatus.currentLap > 2 # if at least in the 3rd lap # termStateErr = (mdl.ParInt[1]*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + # + (1 - mdl.ParInt[1])*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + @NLexpression(mdl.mdl, constZTerm, 100*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) + @NLexpression(mdl.mdl, constZTerm, 100*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) end # Terminal cost # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - # costZTerm = mdl.ParInt[1]*polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]) + - # + (1-mdl.ParInt[1])*polyval(coeffTermCost_1, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); @NLexpression(mdl.mdl, costZTerm, mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) - #@NLexpression(mdl.mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) - # line above not necessary since the polynomial goes to zero anyways! elseif lapStatus.currentLap == 2 # if we're in the second second lap - # costZTerm = polyval(coeffTermCost, nPolyOrderTermCost, ZOlGlobal[Hp*nz+5]); @NLexpression(mdl.mdl, costZTerm, sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) - #@NLexpression(mdl.mdl, costZTerm, costZTerm_h * (0.5-0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) end # State cost @@ -134,6 +129,9 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(mdl.u_Ol),getvalue(mdl.z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) mpcSol.a_x = sol_u[1,1] mpcSol.d_f = sol_u[2,1] + mpcSol.u = sol_u + mpcSol.z = sol_z + mpcSol.cost = [getvalue(costZ) getvalue(costZTerm) getvalue(constZTerm) getvalue(derivCost) getvalue(controlCost) getvalue(laneCost)] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(mdl.z_Ol[1,N+1])) From 753de9096a0a20e09dffab1280bf1b87da2b5ce6 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 19 Sep 2016 20:25:24 -0700 Subject: [PATCH 024/183] Added function to create smoother tracks (continuous derivative of curvature, but kinks allowed --- eval_sim/create_track.py | 37 ++++++++++++++ eval_sim/eval_sim.jl | 51 ++++++++++++++++++- .../src/barc/src/Localization_helpers.py | 38 +++++++++++++- workspace/src/barc/src/Localization_node.py | 5 +- workspace/src/barc/src/helper/functions.jl | 14 +++-- .../src/barc/src/helper/solveMpcProblem.jl | 11 ++-- 6 files changed, 141 insertions(+), 15 deletions(-) create mode 100644 eval_sim/create_track.py diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py new file mode 100644 index 00000000..d8b37169 --- /dev/null +++ b/eval_sim/create_track.py @@ -0,0 +1,37 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.06 +theta = 0 +d_theta = 0 + +N = 40 + +halfcircle = sum(arange(1,N+1,1)) + +for i in range(0,220): + if i < 10: + d_theta = 0 + elif i < 51: + d_theta = d_theta + pi/(2*halfcircle+N) + elif i < 90: + d_theta = d_theta - pi/(2*halfcircle+N) + elif i < 120: + d_theta = 0#d_theta + pi / halfcircle + elif i < 161: + d_theta = d_theta + pi/(2*halfcircle+N) + elif i < 200: + d_theta = d_theta - pi/(2*halfcircle+N) + else: + d_theta = 0 + theta = theta + d_theta + print(d_theta) + #print(theta) + x = hstack((x, x[-1] + cos(theta)*ds)) + y = hstack((y, y[-1] + sin(theta)*ds)) + +plt.plot(x,y,'-o') +plt.axis('equal') +plt.show() \ No newline at end of file diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index bcedb67d..d038e764 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -22,7 +22,10 @@ function eval_sim() z = d["z"] cmd_log = d["cmd_log"] + track = create_track(0.4) + hold(1) plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") + plot(track[:,1],track[:,2],track[:,3],track[:,4],track[:,5],track[:,6]) grid(1) legend(["real state","GPS meas","estimate"]) figure() @@ -48,7 +51,7 @@ function eval_LMPC() sol_u = d["sol_u"] cost = d["cost"] curv = d["curv"] - plot(oldTraj[:,:,1,1]) + plot(oldTraj[:,:,1,1],"-o") grid(1) figure() plot(t,state) @@ -77,7 +80,7 @@ function anim_curv(curv) s = 0.0:.05:2.0 figure() hold(0) - ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + ss = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] for i=1:size(curv,1) c = ss*curv[i,:]' plot(s,c) @@ -93,4 +96,48 @@ function eval_prof() Profile.clear() @load "$(homedir())/simulations/profile.jlprof" ProfileView.view(li, lidict=lidict) +end + +function create_track(w) + x = [0.0] # starting point + y = [0.0] + x_l = [0.0] # starting point + y_l = [w] + x_r = [0.0] # starting point + y_r = [-w] + ds = 0.06 + theta = 0.0 + d_theta = 0.0 + + N = 40 + + halfcircle = sum(1:N) + + for i=0:219 + if i < 10 + d_theta = 0 + elseif i < 51 + d_theta = d_theta + pi/(2*halfcircle+N) + elseif i < 90 + d_theta = d_theta - pi/(2*halfcircle+N) + elseif i < 120 + d_theta = 0#d_theta + pi / halfcircle + elseif i < 161 + d_theta = d_theta + pi/(2*halfcircle+N) + elseif i < 200 + d_theta = d_theta - pi/(2*halfcircle+N) + else + d_theta = 0 + end + theta = theta + d_theta + push!(x, x[end] + cos(theta)*ds) + push!(y, y[end] + sin(theta)*ds) + push!(x_l, x[end-1] + cos(theta+pi/2)*w) + push!(y_l, y[end-1] + sin(theta+pi/2)*w) + push!(x_r, x[end-1] + cos(theta-pi/2)*w) + push!(y_r, y[end-1] + sin(theta-pi/2)*w) + end + track = cat(2, x, y, x_l, y_l, x_r, y_r) + return track + #plot(x,y,x_l,y_l,x_r,y_r) end \ No newline at end of file diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 365bbb07..aad7dab2 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -17,8 +17,8 @@ class Localization: N_nodes_poly_front = 40 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 6 # order of x-y-polynomial interpolation - OrderThetaCurv = 6 # order of theta interpolation + OrderXY = 8 # order of x-y-polynomial interpolation + OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? coeffX = 0 @@ -43,6 +43,40 @@ def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to self.rad = rad #self.ds = rad*2*pi/n self.ds = 2*rad*tan(2*pi/n/2) + def create_track(self): + x = array([0]) # starting point + y = array([0]) + ds = 0.06 + theta = 0 + d_theta = 0 + + N = 40 + + halfcircle = sum(arange(1,N+1,1)) + + for i in range(0,220): + if i < 10: + d_theta = 0 + elif i < 51: + d_theta = d_theta + pi/(2*halfcircle+N) + elif i < 90: + d_theta = d_theta - pi/(2*halfcircle+N) + elif i < 120: + d_theta = 0#d_theta + pi / halfcircle + elif i < 161: + d_theta = d_theta + pi/(2*halfcircle+N) + elif i < 200: + d_theta = d_theta - pi/(2*halfcircle+N) + else: + d_theta = 0 + theta = theta + d_theta + x = hstack((x, x[-1] + cos(theta)*ds)) + y = hstack((y, y[-1] + sin(theta)*ds)) + + self.nodes = array([x,y]) + self.ds = ds + self.n = size(x) + def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines s = size(x) diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 34fb7c94..a259b4c3 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -40,9 +40,10 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) + #l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) + l.create_track() # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) - l.prepare_trajectory(0.063) + #l.prepare_trajectory(0.063) # set node rate loop_rate = 50 diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 7110dc00..efb652b2 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -35,14 +35,14 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.N = 10 mpcParams.nz = 4 mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v - mpcParams.R = [0.0,0.0] # put weights on a and d_f + mpcParams.R = 0.1*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states mpcParams.QderivU = 1*[1,10] # cost matrix for derivative cost of inputs mpcParams.vPathFollowing = 0.8 - trackCoeff.nPolyCurvature = 6 # 4th order polynomial for curvature approximation + trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.width = 0.4 # width of the track (0.5m) + trackCoeff.width = 0.8 # width of the track (0.5m) modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds @@ -55,7 +55,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP modelParams.dt = 0.1 posInfo.s_start = 0 - posInfo.s_target = 10.281192 + posInfo.s_target = 13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) @@ -96,6 +96,12 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara setupperbound(m.u_Ol[i,j], modelParams.u_ub[i,j]) end end + for i=1:4 # I don't know why but somehow the short method returns errors sometimes + for j=1:N+1 + setlowerbound(m.z_Ol[i,j], modelParams.z_lb[i,j]) + setupperbound(m.z_Ol[i,j], modelParams.z_ub[i,j]) + end + end #@variable( m.mdl, 1 >= m.ParInt >= 0 ) @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 04f45bfd..263c4748 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -58,7 +58,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Lane cost # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 10*sum{(0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 10*sum{((0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max)))),i=1:N+1}) # Control Input cost # --------------------------------- @@ -69,11 +69,11 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara if lapStatus.currentLap > 2 # if at least in the 3rd lap # termStateErr = (mdl.ParInt[1]*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + # + (1 - mdl.ParInt[1])*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl.mdl, constZTerm, 100*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; - @NLexpression(mdl.mdl, constZTerm, 100*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) + @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) end # Terminal cost @@ -87,7 +87,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # State cost # --------------------------------- - if lapStatus.currentLap == 1 # if we're in the first lap, just do path following + if lapStatus.currentLap <= 1 # if we're in the first lap, just do path following @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[i,j]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) @@ -131,7 +131,8 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara mpcSol.d_f = sol_u[2,1] mpcSol.u = sol_u mpcSol.z = sol_z - mpcSol.cost = [getvalue(costZ) getvalue(costZTerm) getvalue(constZTerm) getvalue(derivCost) getvalue(controlCost) getvalue(laneCost)] + mpcSol.cost = zeros(6) + #mpcSol.cost = [getvalue(costZ) getvalue(costZTerm) getvalue(constZTerm) getvalue(derivCost) getvalue(controlCost) getvalue(laneCost)] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(mdl.z_Ol[1,N+1])) From 38b48decc52c4674817fa1aabe3ffc33a0db58cc Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 20 Sep 2016 09:26:51 -0700 Subject: [PATCH 025/183] Changed cmd-publish structure so that commands are published at the beginning of an iteration, whereas the state of the iteration beginning is predicted in the previous iteration for the MPC control --- eval_sim/eval_sim.jl | 5 ++-- workspace/src/barc/src/LMPC_node.jl | 30 ++++++++++++------- .../src/barc/src/Localization_helpers.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 1 + .../barc/src/helper/coeffConstraintCost.jl | 10 +++---- workspace/src/barc/src/helper/functions.jl | 27 ++++++++++++++--- .../src/barc/src/helper/solveMpcProblem.jl | 12 ++++---- 7 files changed, 59 insertions(+), 28 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index d038e764..86e42558 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -22,7 +22,7 @@ function eval_sim() z = d["z"] cmd_log = d["cmd_log"] - track = create_track(0.4) + track = create_track(0.2) hold(1) plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") plot(track[:,1],track[:,2],track[:,3],track[:,4],track[:,5],track[:,6]) @@ -51,7 +51,8 @@ function eval_LMPC() sol_u = d["sol_u"] cost = d["cost"] curv = d["curv"] - plot(oldTraj[:,:,1,1],"-o") + plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") + legend([""]) grid(1) figure() plot(t,state) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 3f9c3793..d60f2f52 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -34,8 +34,6 @@ function main() log_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps log_t = zeros(10000,1) log_state = zeros(10000,4) - log_sol_z = zeros(11,4,10000) - log_sol_u = zeros(10,2,10000) log_cost = zeros(10000,6) # Define and initialize variables @@ -54,6 +52,9 @@ function main() InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + log_sol_z = zeros(mpcParams.N+1,4,10000) + log_sol_u = zeros(mpcParams.N,2,10000) + coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) # Initialize ROS node and topics @@ -91,13 +92,16 @@ function main() solve(mdl.mdl) println("Finished.") + pred_z = zeros(4,1) + t0 = time() k = 0 while ! is_shutdown() - if z_est[1] > 0 # check if data has been received (s > 0) - println("Received data: z_est = $z_est") - println("curvature = $coeffCurvature_update") - println("s_start = $s_start_update") + if z_est[1] > 0 # check if data has been received (s > 0) + + # publish command from last calculation + cmd = ECU(mpcSol.a_x, mpcSol.d_f) + publish(pub, cmd) # ============================= Initialize iteration parameters ============================= lapStatus.currentIt += 1 # count iteration @@ -108,6 +112,11 @@ function main() posInfo.s_start = s_start_update[1] trackCoeff.coeffCurvature = coeffCurvature_update + # Simulate model for next input + simModel(pred_z,zCurr[i,:]',uCurr[i,:]',modelParams.dt,modelParams.l_A,modelParams.l_B,trackCoeff.coeffCurvature) + # this simulation returns the predicted state at when the next command is going to be sent. This predicted state is used for + # the MPC control input calculation. + # ======================================= Lap trigger ======================================= # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! @@ -141,7 +150,7 @@ function main() println("======================================== NEW ITERATION # $i ========================================") println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") println("State Nr. $i = $z_est") - println("Coeff Curvature = $(trackCoeff.coeffCurvature)") + #println("Coeff Curvature = $(trackCoeff.coeffCurvature)") println("posInfo = $posInfo") println("s = $(posInfo.s)") println("s_start = $(posInfo.s_start)") @@ -157,15 +166,15 @@ function main() # Solve the MPC problem tic() - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') + tt = toq() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] #println("trackCoeff = $trackCoeff") println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") # ... and publish data - cmd = ECU(mpcSol.a_x, mpcSol.d_f) - publish(pub, cmd) zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) println("\n") @@ -178,6 +187,7 @@ function main() log_sol_u[:,:,k] = mpcSol.u' log_cost[k,:] = mpcSol.cost log_curv[k,:] = trackCoeff.coeffCurvature + println("pred_z = $pred_z") else println("No estimation data received!") diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index aad7dab2..693663c5 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -235,7 +235,7 @@ def find_s(self): # Calculate s - discretization = 0.01 # discretization to calculate s + discretization = 0.001 # discretization to calculate s s_idx_start = max(0,idx_min-1) # where the discretization starts if self.closed and s_idx_start-idx_start<0: s_idx_start = s_idx_start + n diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index b5f2f39e..6fe3a6f2 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -102,6 +102,7 @@ function main() s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) z_current = zeros(60000,4) + z_current[1,:] = [0.1 0 0 0] dt = 0.01 loop_rate = Rate(1/dt) diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index 9d3a630d..c8193f67 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -72,9 +72,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo end # bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) - println("************************************** COEFFICIENTS **************************************") - println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") - println("s_total = $s_total") + # println("************************************** COEFFICIENTS **************************************") + # println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") + # println("s_total = $s_total") # println("bS_Vector[1] = $(bS_Vector[:,:,1]')") # These matrices (above) contain two vectors each (for both old trajectories), stored in the 3rd dimension @@ -85,7 +85,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo if s_total - s_start < 0 s_forinterpy += s_target end - println("s_forinterpy[:,1,1]' = $(s_forinterpy[:,1,1]')") + # println("s_forinterpy[:,1,1]' = $(s_forinterpy[:,1,1]')") # Create the Matrices for the interpolation MatrixInterp = zeros(pLength+1,Order+1,2) @@ -114,7 +114,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position idx_s_target = find(oldS[:,i].>s_target)[1] dist_to_s_target = (idx_s_target - IndexBezierS) - println("dist_to_s_target $i = $dist_to_s_target") + # println("dist_to_s_target $i = $dist_to_s_target") #dist_to_s_target = dist_to_s_target + 30 bQfunction_Vector = zeros(pLength+1,1) diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index efb652b2..5bd4a529 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -32,17 +32,17 @@ end function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) - mpcParams.N = 10 + mpcParams.N = 15 mpcParams.nz = 4 mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v mpcParams.R = 0.1*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states mpcParams.QderivU = 1*[1,10] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 0.8 + mpcParams.vPathFollowing = 1.0 trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.width = 0.8 # width of the track (0.5m) + trackCoeff.width = 0.4 # width of the track (0.5m) modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds @@ -84,7 +84,7 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation - m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.1))#,linear_solver="ma57",print_user_options="yes")) + m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) @variable( m.mdl, m.z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v @variable( m.mdl, m.u_Ol[1:2,1:N]) @@ -122,4 +122,23 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara @NLconstraint(m.mdl, m.z_Ol[4,i+1] == m.z_Ol[4,i] + dt*(c0[1]*m.u_Ol[1,i] - c0[2]*abs(m.z_Ol[4,i]) * m.z_Ol[4,i])) # v end +end + +function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},dt::Float64,l_A::Float64,l_B::Float64,coeff::Array{Float64}) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + s = z[1] + c = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s 1] * coeff'')[1] + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + dsdt = z[4] *cos(z[3]+bta)/(1-z[2]*c) + + zNext[1] = z[1] + dt*dsdt # s + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_A*sin(bta)-dsdt*c) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + + nothing end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 263c4748..862ce982 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -29,11 +29,11 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara sol_u::Array{Float64,2} sol_z::Array{Float64,2} - println("************************************** MPC SOLVER **************************************") - println("zCurr = $(zCurr')") - println("s_start = $s_start") - println("s_target = $s_target") - println("s_total = $((zCurr[1]+s_start)%s_target)") + # println("************************************** MPC SOLVER **************************************") + # println("zCurr = $(zCurr')") + # println("s_start = $s_start") + # println("s_target = $s_target") + # println("s_total = $((zCurr[1]+s_start)%s_target)") # Create function-specific parameters z_Ref::Array{Float64,2} @@ -58,7 +58,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Lane cost # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 10*sum{((0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max)))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 10*sum{mdl.z_Ol[2,i]^2*((0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max)))),i=1:N+1}) # Control Input cost # --------------------------------- From 6d15f5b696b55b945bf402fcf84b943ff3ef3d64 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 20 Sep 2016 14:23:20 -0700 Subject: [PATCH 026/183] Added new method to model racetrack (by stacking curves together). Changed racetrack to more complicated path. --- eval_sim/create_track.py | 60 ++++++++------ eval_sim/eval_sim.jl | 79 +++++++++++-------- workspace/src/barc/src/LMPC_node.jl | 4 + .../src/barc/src/Localization_helpers.py | 57 +++++++------ workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 2 +- .../barc/src/helper/coeffConstraintCost.jl | 2 +- workspace/src/barc/src/helper/functions.jl | 29 ++++--- .../src/barc/src/helper/solveMpcProblem.jl | 15 ++-- 9 files changed, 148 insertions(+), 102 deletions(-) diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index d8b37169..905a83f0 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -9,29 +9,45 @@ N = 40 -halfcircle = sum(arange(1,N+1,1)) - -for i in range(0,220): - if i < 10: - d_theta = 0 - elif i < 51: - d_theta = d_theta + pi/(2*halfcircle+N) - elif i < 90: - d_theta = d_theta - pi/(2*halfcircle+N) - elif i < 120: - d_theta = 0#d_theta + pi / halfcircle - elif i < 161: - d_theta = d_theta + pi/(2*halfcircle+N) - elif i < 200: - d_theta = d_theta - pi/(2*halfcircle+N) - else: - d_theta = 0 - theta = theta + d_theta - print(d_theta) - #print(theta) - x = hstack((x, x[-1] + cos(theta)*ds)) - y = hstack((y, y[-1] + sin(theta)*ds)) +def add_curve(theta, length, angle): + d_theta = 0 + curve = 2*sum(arange(1,length/2+1,1))+length/2 + for i in range(0,length): + if i < length/2+1: + d_theta = d_theta + angle / curve + else: + d_theta = d_theta - angle / curve + theta = hstack((theta,theta[-1] + d_theta)) + return theta + + +theta = array([0]) + +theta = add_curve(theta,10,0) +theta = add_curve(theta,50,-2*pi/3) +#theta = add_curve(theta,30,0) +theta = add_curve(theta,70,pi) +theta = add_curve(theta,60,-5*pi/6) +theta = add_curve(theta,10,0) +theta = add_curve(theta,30,-pi/2) +theta = add_curve(theta,40,0) +theta = add_curve(theta,20,-pi/4) +theta = add_curve(theta,20,pi/4) +theta = add_curve(theta,50,-pi/2) +theta = add_curve(theta,22,0) +theta = add_curve(theta,30,-pi/2) +theta = add_curve(theta,14,0) +#theta = add_curve(theta,7,0) +#theta = add_curve(theta,30,3*pi/4) +#theta = add_curve(theta,10,-1*pi/4) +#theta = add_curve(theta,20,-pi/2) +#theta = add_curve(theta,20,0) + +for i in range(0,size(theta)): + x = hstack((x, x[-1] + cos(theta[i])*ds)) + y = hstack((y, y[-1] + sin(theta[i])*ds)) plt.plot(x,y,'-o') plt.axis('equal') +plt.grid() plt.show() \ No newline at end of file diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 86e42558..46b6b20f 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -25,7 +25,7 @@ function eval_sim() track = create_track(0.2) hold(1) plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") - plot(track[:,1],track[:,2],track[:,3],track[:,4],track[:,5],track[:,6]) + plot(track[:,1],track[:,2],"b-",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) legend(["real state","GPS meas","estimate"]) figure() @@ -52,17 +52,21 @@ function eval_LMPC() cost = d["cost"] curv = d["curv"] plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") - legend([""]) + legend(["e_y","e_psi","v"]) grid(1) figure() + ax1=subplot(211) plot(t,state) + legend(["s","e_y","e_psi","v"]) grid(1) + #figure() + subplot(212,sharex = ax1) plot(t,cost) grid(1) legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) figure() plot(t,curv) - legend(["1","2","3","4","5","6","7"]) + legend(["1","2","3","4","5","6","7","8","9"]) end function anim_MPC(z) @@ -107,38 +111,45 @@ function create_track(w) x_r = [0.0] # starting point y_r = [-w] ds = 0.06 - theta = 0.0 - d_theta = 0.0 - - N = 40 - - halfcircle = sum(1:N) - - for i=0:219 - if i < 10 - d_theta = 0 - elseif i < 51 - d_theta = d_theta + pi/(2*halfcircle+N) - elseif i < 90 - d_theta = d_theta - pi/(2*halfcircle+N) - elseif i < 120 - d_theta = 0#d_theta + pi / halfcircle - elseif i < 161 - d_theta = d_theta + pi/(2*halfcircle+N) - elseif i < 200 - d_theta = d_theta - pi/(2*halfcircle+N) - else - d_theta = 0 - end - theta = theta + d_theta - push!(x, x[end] + cos(theta)*ds) - push!(y, y[end] + sin(theta)*ds) - push!(x_l, x[end-1] + cos(theta+pi/2)*w) - push!(y_l, y[end-1] + sin(theta+pi/2)*w) - push!(x_r, x[end-1] + cos(theta-pi/2)*w) - push!(y_r, y[end-1] + sin(theta-pi/2)*w) + + theta = [0.0] + + add_curve(theta,10,0.0) + add_curve(theta,50,-2*pi/3) + add_curve(theta,70,pi) + add_curve(theta,60,-5*pi/6) + add_curve(theta,10,0.0) + add_curve(theta,30,-pi/2) + add_curve(theta,40,0.0) + add_curve(theta,20,-pi/4) + add_curve(theta,20,pi/4) + add_curve(theta,50,-pi/2) + add_curve(theta,22,0.0) + add_curve(theta,30,-pi/2) + add_curve(theta,14,0.0) + + for i=1:length(theta) + push!(x, x[end] + cos(theta[i])*ds) + push!(y, y[end] + sin(theta[i])*ds) + push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) + push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) + push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) + push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) end track = cat(2, x, y, x_l, y_l, x_r, y_r) return track #plot(x,y,x_l,y_l,x_r,y_r) -end \ No newline at end of file +end + +function add_curve(theta::Array{Float64}, length::Int64, angle) + d_theta = 0 + curve = 2*sum(1:length/2)+length/2 + for i=0:length-1 + if i < length/2+1 + d_theta = d_theta + angle / curve + else + d_theta = d_theta - angle / curve + end + push!(theta, theta[end] + d_theta) + end +end diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index d60f2f52..fc5eba10 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -145,6 +145,10 @@ function main() switchLap = true end + # if we are at least in the 2nd lap, concatenate the beginning to the end of the previous track + if lapStatus.currentLap > 1 && lapStatus.currentIt == 80 + extendOldTraj(oldTraj,posInfo,zCurr) + end # ======================================= Calculate input ======================================= println("======================================== NEW ITERATION # $i ========================================") diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 693663c5..093c515c 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -7,6 +7,17 @@ def create_circle(rad,n,c): y = rad*sin(ang)+c[1] return array([x,y]) +def add_curve(theta, length, angle): + d_theta = 0 + curve = 2*sum(arange(1,length/2+1,1))+length/2 + for i in range(0,length): + if i < length/2+1: + d_theta = d_theta + angle / curve + else: + d_theta = d_theta - angle / curve + theta = hstack((theta,theta[-1] + d_theta)) + return theta + class Localization: n = 0 # number of nodes c = 0 # center of circle (in case of circular trajectory) @@ -47,35 +58,31 @@ def create_track(self): x = array([0]) # starting point y = array([0]) ds = 0.06 - theta = 0 - d_theta = 0 - - N = 40 - - halfcircle = sum(arange(1,N+1,1)) - - for i in range(0,220): - if i < 10: - d_theta = 0 - elif i < 51: - d_theta = d_theta + pi/(2*halfcircle+N) - elif i < 90: - d_theta = d_theta - pi/(2*halfcircle+N) - elif i < 120: - d_theta = 0#d_theta + pi / halfcircle - elif i < 161: - d_theta = d_theta + pi/(2*halfcircle+N) - elif i < 200: - d_theta = d_theta - pi/(2*halfcircle+N) - else: - d_theta = 0 - theta = theta + d_theta - x = hstack((x, x[-1] + cos(theta)*ds)) - y = hstack((y, y[-1] + sin(theta)*ds)) + theta = array([0]) + + theta = add_curve(theta,10,0) + theta = add_curve(theta,50,-2*pi/3) + theta = add_curve(theta,70,pi) + theta = add_curve(theta,60,-5*pi/6) + theta = add_curve(theta,10,0) + theta = add_curve(theta,30,-pi/2) + theta = add_curve(theta,40,0) + theta = add_curve(theta,20,-pi/4) + theta = add_curve(theta,20,pi/4) + theta = add_curve(theta,50,-pi/2) + theta = add_curve(theta,22,0) + theta = add_curve(theta,30,-pi/2) + theta = add_curve(theta,14,0) + + for i in range(0,size(theta)): + x = hstack((x, x[-1] + cos(theta[i])*ds)) + y = hstack((y, y[-1] + sin(theta[i])*ds)) self.nodes = array([x,y]) self.ds = ds self.n = size(x) + print "number of nodes: %i"%self.n + print "length : %f"%((self.n-1)*ds) def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index a259b4c3..a7f6611d 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -43,7 +43,7 @@ def localization_node(): #l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) l.create_track() # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) - #l.prepare_trajectory(0.063) + # l.prepare_trajectory(0.063) # set node rate loop_rate = 50 diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 6fe3a6f2..82e3a9e8 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -64,7 +64,7 @@ function simModel(z,u,dt,l_A,l_B) zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v # Add process noise (depending on velocity) - zNext = zNext + 0.0*diagm([0.01*z[4],0.01*z[4],0.001,0.01*z[4]])*randn(4,1) + zNext = zNext + 0*diagm([0.01*z[4],0.01*z[4],0.001,0.01*z[4]])*randn(4,1) return zNext end diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index c8193f67..e5b36ea5 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -115,7 +115,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo idx_s_target = find(oldS[:,i].>s_target)[1] dist_to_s_target = (idx_s_target - IndexBezierS) # println("dist_to_s_target $i = $dist_to_s_target") - #dist_to_s_target = dist_to_s_target + 30 + dist_to_s_target = dist_to_s_target + 30 bQfunction_Vector = zeros(pLength+1,1) # Select the part needed for the interpolation diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 5bd4a529..22656f9a 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -32,7 +32,7 @@ end function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) - mpcParams.N = 15 + mpcParams.N = 10 mpcParams.nz = 4 mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v mpcParams.R = 0.1*[1.0,1.0] # put weights on a and d_f @@ -45,7 +45,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP trackCoeff.width = 0.4 # width of the track (0.5m) modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering - modelParams.u_ub = [1.0 pi/6]' * ones(1,mpcParams.N) # upper bounds + modelParams.u_ub = [2.0 pi/6]' * ones(1,mpcParams.N) # upper bounds modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) @@ -55,7 +55,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP modelParams.dt = 0.1 posInfo.s_start = 0 - posInfo.s_target = 13.20#10.281192 + posInfo.s_target = 25.62#13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) @@ -96,12 +96,12 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara setupperbound(m.u_Ol[i,j], modelParams.u_ub[i,j]) end end - for i=1:4 # I don't know why but somehow the short method returns errors sometimes - for j=1:N+1 - setlowerbound(m.z_Ol[i,j], modelParams.z_lb[i,j]) - setupperbound(m.z_Ol[i,j], modelParams.z_ub[i,j]) - end - end + # for i=1:4 + # for j=1:N+1 + # setlowerbound(m.z_Ol[i,j], modelParams.z_lb[i,j]) + # setupperbound(m.z_Ol[i,j], modelParams.z_ub[i,j]) + # end + # end #@variable( m.mdl, 1 >= m.ParInt >= 0 ) @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) @@ -141,4 +141,15 @@ function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},dt:: zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v nothing +end + +function extendOldTraj(oldTraj::OldTrajectory,posInfo::PosInfo,zCurr::Array{Float64,2}) + #println(size(zCurr[1:21,:])) + #println(size(oldTraj.oldTraj[oldTraj.oldCost[1]:oldTraj.oldCost[1]+20,1:4,1])) + for i=1:4 + for j=1:50 + oldTraj.oldTraj[oldTraj.oldCost[1]+j-1,i,1] = zCurr[j,i] + end + end + oldTraj.oldTraj[oldTraj.oldCost[1]:oldTraj.oldCost[1]+49,1,1] += posInfo.s_target end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 862ce982..74885503 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -58,7 +58,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Lane cost # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 10*sum{mdl.z_Ol[2,i]^2*((0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max)))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 100*sum{mdl.z_Ol[2,i]^2*((0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max)))),i=1:N+1}) # Control Input cost # --------------------------------- @@ -67,22 +67,20 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - # termStateErr = (mdl.ParInt[1]*polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) + - # + (1 - mdl.ParInt[1])*polyval(coeffTermCons_1[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5])) - ZOlGlobal[Hp*nz+j]; @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap - # termStateErr = polyval(coeffTermCons[j], nPolyOrderTermCons, ZOlGlobal[Hp*nz+5]) - ZOlGlobal[Hp*nz+j]; @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) end # Terminal cost # --------------------------------- + # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line. if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, costZTerm, mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl.mdl, costZTerm, 1*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ + (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1})) elseif lapStatus.currentLap == 2 # if we're in the second second lap - @NLexpression(mdl.mdl, costZTerm, sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl.mdl, costZTerm, 1*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) end # State cost @@ -94,7 +92,6 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara #@NLexpression(mdl.mdl, costZ_h, 0) # zero state cost after crossing the finish line #@NLexpression(mdl.mdl, costZ, 1 + (costZ_h-1) * (0.5+0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) @NLexpression(mdl.mdl, costZ, 1) - #@NLexpression(mdl.mdl, costZ, 1) end @NLobjective(mdl.mdl, Min, costZ + costZTerm + constZTerm + derivCost + controlCost + laneCost) @@ -132,7 +129,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara mpcSol.u = sol_u mpcSol.z = sol_z mpcSol.cost = zeros(6) - #mpcSol.cost = [getvalue(costZ) getvalue(costZTerm) getvalue(constZTerm) getvalue(derivCost) getvalue(controlCost) getvalue(laneCost)] + #mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(mdl.z_Ol[1,N+1])) From e06975cfe4d4a6c2c93e60c4647e6f9b75f7919a Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 20 Sep 2016 18:07:38 -0700 Subject: [PATCH 027/183] Added method to create racetracks with Bezier curves. --- eval_sim/create_track_bezier.py | 54 +++++++++++++++++++++++++++++++++ eval_sim/eval_sim.jl | 16 +++++++--- 2 files changed, 66 insertions(+), 4 deletions(-) create mode 100644 eval_sim/create_track_bezier.py diff --git a/eval_sim/create_track_bezier.py b/eval_sim/create_track_bezier.py new file mode 100644 index 00000000..c5fe3dd8 --- /dev/null +++ b/eval_sim/create_track_bezier.py @@ -0,0 +1,54 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.06 +theta = 0 +d_theta = 0 + +N = 40 + +def create_bezier(p0,p1,p2,dt): + t = arange(0,1+dt,dt)[:,None] + p = (1-t)**2*p0 + 2*(1-t)*t*p1+t**2*p2 + return p + +p0 = array([[0,0], + [2,0], + [2,-2], + [1,-4], + [3,-4], + [5,-5], + [3,-6], + [1,-6], + [-0.5,-5.5], + [-1.5,-5], + [-3,-4], + [-3,-2], + [0,0]]) +p1 = array([0,0,0.75,-1,1,inf,0,0,-1,0,inf,inf,0]) + +p = p0[0,:] +for i in range(0,size(p0,0)-1): + b1 = p0[i,1] - p1[i]*p0[i,0] + b2 = p0[i+1,1] - p1[i+1]*p0[i+1,0] + a1 = p1[i] + a2 = p1[i+1] + x = (b2-b1)/(a1-a2) + y = a1*x + b1 + if p1[i] == inf: + x = p0[i,0] + y = a2*x+b2 + elif p1[i+1] == inf: + x = p0[i+1,0] + y = a1*x+b1 + if a1 == a2: + p = vstack((p,p0[i+1,:])) + else: + p = vstack((p,create_bezier(p0[i,:],array([[x,y]]),p0[i+1,:],0.01))) + +plt.plot(p[:,0],p[:,1],'-',p0[:,0],p0[:,1],'*') +plt.grid() +plt.axis('equal') +plt.show() diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 46b6b20f..fec19617 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -23,9 +23,10 @@ function eval_sim() cmd_log = d["cmd_log"] track = create_track(0.2) + figure() hold(1) plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") - plot(track[:,1],track[:,2],"b-",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) legend(["real state","GPS meas","estimate"]) figure() @@ -69,6 +70,12 @@ function eval_LMPC() legend(["1","2","3","4","5","6","7","8","9"]) end +function eval_oldTraj(i) + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + plot(oldTraj[:,1,1,i],oldTraj[:,2:4,1,i],"-o",oldTraj[:,1,2,i],oldTraj[:,2:4,2,i],"-*") +end + function anim_MPC(z) figure() hold(0) @@ -77,7 +84,7 @@ function anim_MPC(z) plot(z[:,:,i]) xlim([1,11]) ylim([-2,2]) - sleep(0.1) + sleep(0.01) end end @@ -85,13 +92,14 @@ function anim_curv(curv) s = 0.0:.05:2.0 figure() hold(0) - ss = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + #ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] for i=1:size(curv,1) c = ss*curv[i,:]' plot(s,c) xlim([0,2]) ylim([-1.5,1.5]) - sleep(0.25) + sleep(0.1) end end From 7752f6e4d0ede9843fb3056a430c63a3ada6390e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 20 Sep 2016 18:22:18 -0700 Subject: [PATCH 028/183] Added Bezier function to create racetracks (didn't work at previous commit), transposed MPC matrices --- workspace/src/barc/src/LMPC_node.jl | 10 ++-- .../src/barc/src/Localization_helpers.py | 56 +++++++++++++++++-- workspace/src/barc/src/Localization_node.py | 4 +- workspace/src/barc/src/helper/functions.jl | 38 +++++++------ .../src/barc/src/helper/solveMpcProblem.jl | 29 +++++----- 5 files changed, 94 insertions(+), 43 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index fc5eba10..de1ce00f 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -113,7 +113,7 @@ function main() trackCoeff.coeffCurvature = coeffCurvature_update # Simulate model for next input - simModel(pred_z,zCurr[i,:]',uCurr[i,:]',modelParams.dt,modelParams.l_A,modelParams.l_B,trackCoeff.coeffCurvature) + simModel(pred_z,zCurr[i,:]',uCurr[i,:]',modelParams.dt,modelParams.l_A,modelParams.l_B,trackCoeff) # this simulation returns the predicted state at when the next command is going to be sent. This predicted state is used for # the MPC control input calculation. @@ -125,7 +125,7 @@ function main() println("Saving data") tic() saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) - log_oldTraj[1:i,:,:,lapStatus.currentLap] = oldTraj.oldTraj[1:i,:,:] + log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state uCurr[1,:] = uCurr[i+1,:] # ... and input i = 1 @@ -147,7 +147,7 @@ function main() # if we are at least in the 2nd lap, concatenate the beginning to the end of the previous track if lapStatus.currentLap > 1 && lapStatus.currentIt == 80 - extendOldTraj(oldTraj,posInfo,zCurr) + #extendOldTraj(oldTraj,posInfo,zCurr) end # ======================================= Calculate input ======================================= @@ -187,8 +187,8 @@ function main() k = k + 1 # counter log_state[k,:] = z_est log_t[k] = time() - t0 - log_sol_z[:,:,k] = mpcSol.z' - log_sol_u[:,:,k] = mpcSol.u' + log_sol_z[:,:,k] = mpcSol.z + log_sol_u[:,:,k] = mpcSol.u log_cost[k,:] = mpcSol.cost log_curv[k,:] = trackCoeff.coeffCurvature println("pred_z = $pred_z") diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 093c515c..d0e5de63 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -18,6 +18,11 @@ def add_curve(theta, length, angle): theta = hstack((theta,theta[-1] + d_theta)) return theta +def create_bezier(p0,p1,p2,dt): + t = arange(0,1+dt,dt)[:,None] + p = (1-t)**2*p0 + 2*(1-t)*t*p1+t**2*p2 + return p + class Localization: n = 0 # number of nodes c = 0 # center of circle (in case of circular trajectory) @@ -28,8 +33,8 @@ class Localization: N_nodes_poly_front = 40 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 8 # order of x-y-polynomial interpolation - OrderThetaCurv = 8 # order of theta interpolation + OrderXY = 6 # order of x-y-polynomial interpolation + OrderThetaCurv = 6 # order of theta interpolation closed = True # open or closed trajectory? coeffX = 0 @@ -54,6 +59,44 @@ def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to self.rad = rad #self.ds = rad*2*pi/n self.ds = 2*rad*tan(2*pi/n/2) + + def create_track2(self): + p0 = array([[0,0], + [2,0], + [2,-2], + [1,-4], + [3,-4], + [5,-5], + [3,-6], + [1,-6], + [-0.5,-5.5], + [-1.5,-5], + [-3,-4], + [-3,-2], + [0,0]]) + p1 = array([0,0,0.75,-1,1,inf,0,0,-1,0,inf,inf,0]) + + p = p0[0,:] + for i in range(0,size(p0,0)-1): + b1 = p0[i,1] - p1[i]*p0[i,0] + b2 = p0[i+1,1] - p1[i+1]*p0[i+1,0] + a1 = p1[i] + a2 = p1[i+1] + x = (b2-b1)/(a1-a2) + y = a1*x + b1 + if p1[i] == inf: + x = p0[i,0] + y = a2*x+b2 + elif p1[i+1] == inf: + x = p0[i+1,0] + y = a1*x+b1 + if a1 == a2: + p = vstack((p,p0[i+1,:])) + else: + p = vstack((p,create_bezier(p0[i,:],array([[x,y]]),p0[i+1,:],0.01))) + self.nodes = transpose(p) + self.ds = 0.06 + def create_track(self): x = array([0]) # starting point y = array([0]) @@ -128,11 +171,11 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e length = sum(ds) dsn = ds_in # optimal new step size (might be calculated externally for now) rem = lambda x:mod(length,x) # function to calculate optimal step size (for evenly distributed steps) - sol = scipy.optimize.fmin(func=rem,x0=dsn) + sol = scipy.optimize.fmin(func=rem,x0=dsn,args=(),xtol=0.0000001,ftol=0.0000001) dsn = sol[0] #sn = arange(0,length,dsn) # new steps #n = size(sn,0) - n = floor(length/dsn)+1 + n = floor(length/dsn) #print(n) sn = arange(0,n*dsn,dsn) #print(sn) @@ -148,6 +191,7 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e self.nodes = array([xn,yn]) self.ds = dsn self.n = size(xn) + print "Finished track optimization." print "Track length = %fm, ds = %fm"%(length,dsn) print "Approximated length = %fm"%(self.nPoints*dsn) #print(sn) @@ -235,7 +279,7 @@ def find_s(self): coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] - #if max(coeffCurvature) > 50: + # if max(coeffCurvature) > 200: # print "Large value, idx_min = %f"%(idx_min) # print(nodes_X) # print(nodes_Y) @@ -243,7 +287,7 @@ def find_s(self): # Calculate s discretization = 0.001 # discretization to calculate s - s_idx_start = max(0,idx_min-1) # where the discretization starts + s_idx_start = max(0,idx_min-1) # where the discretization starts if self.closed and s_idx_start-idx_start<0: s_idx_start = s_idx_start + n j = arange((s_idx_start-idx_start)*self.ds,(s_idx_start-idx_start+2)*self.ds,discretization) diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index a7f6611d..72f7c873 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -12,6 +12,7 @@ from Localization_helpers import Localization from barc.msg import Z_KinBkMdl, pos_info + l = 0 epsi_prev = 0 @@ -42,8 +43,9 @@ def localization_node(): # l.create_circle(1,100,array([3.2,0.5])) #l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) l.create_track() + #l.create_track2() # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) - # l.prepare_trajectory(0.063) + l.prepare_trajectory(0.06) # set node rate loop_rate = 50 diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 22656f9a..5d9a4e9f 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -38,9 +38,9 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.R = 0.1*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states mpcParams.QderivU = 1*[1,10] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 1.0 + mpcParams.vPathFollowing = 0.5 - trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation + trackCoeff.nPolyCurvature = 6 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.width = 0.4 # width of the track (0.5m) @@ -55,7 +55,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP modelParams.dt = 0.1 posInfo.s_start = 0 - posInfo.s_target = 25.62#13.20#10.281192 + posInfo.s_target = 25.62#29.491949#13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) @@ -86,14 +86,14 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) - @variable( m.mdl, m.z_Ol[1:4,1:(N+1)]) # z = s, ey, epsi, v - @variable( m.mdl, m.u_Ol[1:2,1:N]) + @variable( m.mdl, m.z_Ol[1:(N+1),1:4]) # z = s, ey, epsi, v + @variable( m.mdl, m.u_Ol[1:N,1:2]) @variable( m.mdl, 0 <= m.ParInt[1:1] <= 1) for i=1:2 # I don't know why but somehow the short method returns errors sometimes for j=1:N - setlowerbound(m.u_Ol[i,j], modelParams.u_lb[i,j]) - setupperbound(m.u_Ol[i,j], modelParams.u_ub[i,j]) + setlowerbound(m.u_Ol[j,i], modelParams.u_lb[i,j]) + setupperbound(m.u_Ol[j,i], modelParams.u_ub[i,j]) end end # for i=1:4 @@ -105,33 +105,37 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara #@variable( m.mdl, 1 >= m.ParInt >= 0 ) @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) - @NLconstraint(m.mdl, [i=1:4], m.z_Ol[i,1] == m.z0[i]) + @NLconstraint(m.mdl, [i=1:4], m.z_Ol[1,i] == m.z0[i]) @NLparameter(m.mdl, m.coeff[i=1:n_poly_curv+1] == trackCoeff.coeffCurvature[i]); #@NLexpression(m.mdl, m.c[i = 1:N], m.coeff[1]*m.z_Ol[1,i]^4+m.coeff[2]*m.z_Ol[1,i]^3+m.coeff[3]*m.z_Ol[1,i]^2+m.coeff[4]*m.z_Ol[1,i]+m.coeff[5]) - @NLexpression(m.mdl, m.c[i = 1:N], sum{m.coeff[j]*m.z_Ol[1,i]^(n_poly_curv-j+1),j=1:n_poly_curv} + m.coeff[n_poly_curv+1]) - @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( m.u_Ol[2,i] ) ) ) - @NLexpression(m.mdl, m.dsdt[i = 1:N], m.z_Ol[4,i]*cos(m.z_Ol[3,i]+m.bta[i])/(1-m.z_Ol[2,i]*m.c[i])) + @NLexpression(m.mdl, m.c[i = 1:N], sum{m.coeff[j]*m.z_Ol[i,1]^(n_poly_curv-j+1),j=1:n_poly_curv} + m.coeff[n_poly_curv+1]) + @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( m.u_Ol[i,2] ) ) ) + @NLexpression(m.mdl, m.dsdt[i = 1:N], m.z_Ol[i,4]*cos(m.z_Ol[i,3]+m.bta[i])/(1-m.z_Ol[i,2]*m.c[i])) # System dynamics for i=1:N - @NLconstraint(m.mdl, m.z_Ol[1,i+1] == m.z_Ol[1,i] + dt*m.dsdt[i] ) # s - @NLconstraint(m.mdl, m.z_Ol[2,i+1] == m.z_Ol[2,i] + dt*m.z_Ol[4,i]*sin(m.z_Ol[3,i]+m.bta[i]) ) # ey - @NLconstraint(m.mdl, m.z_Ol[3,i+1] == m.z_Ol[3,i] + dt*(m.z_Ol[4,i]/L_a*sin(m.bta[i])-m.dsdt[i]*m.c[i]) ) # epsi - @NLconstraint(m.mdl, m.z_Ol[4,i+1] == m.z_Ol[4,i] + dt*(c0[1]*m.u_Ol[1,i] - c0[2]*abs(m.z_Ol[4,i]) * m.z_Ol[4,i])) # v + @NLconstraint(m.mdl, m.z_Ol[i+1,1] == m.z_Ol[i,1] + dt*m.dsdt[i] ) # s + @NLconstraint(m.mdl, m.z_Ol[i+1,2] == m.z_Ol[i,2] + dt*m.z_Ol[i,4]*sin(m.z_Ol[i,3]+m.bta[i]) ) # ey + @NLconstraint(m.mdl, m.z_Ol[i+1,3] == m.z_Ol[i,3] + dt*(m.z_Ol[i,4]/L_a*sin(m.bta[i])-m.dsdt[i]*m.c[i]) ) # epsi + @NLconstraint(m.mdl, m.z_Ol[i+1,4] == m.z_Ol[i,4] + dt*(c0[1]*m.u_Ol[i,1] - c0[2]*abs(m.z_Ol[i,4]) * m.z_Ol[i,4])) # v end end -function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},dt::Float64,l_A::Float64,l_B::Float64,coeff::Array{Float64}) +function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},dt::Float64,l_A::Float64,l_B::Float64,trackCoeff::TrackCoeff) # kinematic bicycle model # u[1] = acceleration # u[2] = steering angle s = z[1] - c = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s 1] * coeff'')[1] + c = 0 + for i=trackCoeff.nPolyCurvature:-1:0 + c += s^i * trackCoeff.coeffCurvature[trackCoeff.nPolyCurvature+1-i] + end + #c = ([s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s 1] * coeff'')[1] bta = atan(l_A/(l_A+l_B)*tan(u[2])) dsdt = z[4] *cos(z[3]+bta)/(1-z[2]*c) diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 74885503..983ad3cd 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -53,40 +53,40 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Derivative cost # --------------------------------- - @NLexpression(mdl.mdl, derivCost, 0*sum{QderivZ[j]*((zCurr[j]-mdl.z_Ol[j,1])^2+sum{(mdl.z_Ol[j,i]-mdl.z_Ol[j,i+1])^2,i=1:N}),j=1:4} + - sum{QderivU[j]*((uCurr[j]-mdl.u_Ol[j,1])^2+sum{(mdl.u_Ol[j,i]-mdl.u_Ol[j,i+1])^2,i=1:N-1}),j=1:2}) + @NLexpression(mdl.mdl, derivCost, 0*sum{QderivZ[j]*((zCurr[j]-mdl.z_Ol[1,j])^2+sum{(mdl.z_Ol[i,j]-mdl.z_Ol[i+1,j])^2,i=1:N}),j=1:4} + + sum{QderivU[j]*((uCurr[j]-mdl.u_Ol[1,j])^2+sum{(mdl.u_Ol[i,j]-mdl.u_Ol[i+1,j])^2,i=1:N-1}),j=1:2}) # Lane cost # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 100*sum{mdl.z_Ol[2,i]^2*((0.5+0.5*tanh(50*(mdl.z_Ol[2,i]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[2,i]+ey_max)))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 100*sum{mdl.z_Ol[i,2]^2*((0.5+0.5*tanh(50*(mdl.z_Ol[i,2]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[i,2]+ey_max)))),i=1:N+1}) # Control Input cost # --------------------------------- - @NLexpression(mdl.mdl, controlCost, 0.5*sum{R[j]*sum{(mdl.u_Ol[j,i]-u_Ref[i,j])^2,i=1:N},j=1:2}) + @NLexpression(mdl.mdl, controlCost, 0.5*sum{R[j]*sum{(mdl.u_Ol[i,j]-u_Ref[i,j])^2,i=1:N},j=1:2}) # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3})) + @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ + (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap - @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}-mdl.z_Ol[j+1,N+1])^2,j=1:3}) + @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3}) end # Terminal cost # --------------------------------- # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line. if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, costZTerm, 1*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}+ - (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1})) + @NLexpression(mdl.mdl, costZTerm, 5*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ + (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1})) elseif lapStatus.currentLap == 2 # if we're in the second second lap - @NLexpression(mdl.mdl, costZTerm, 1*sum{coeffTermCost[i,1]*mdl.z_Ol[1,N+1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl.mdl, costZTerm, 5*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}) end # State cost # --------------------------------- if lapStatus.currentLap <= 1 # if we're in the first lap, just do path following - @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[i,j]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory + @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[j,i]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) #@NLexpression(mdl.mdl, costZ_h, 0) # zero state cost after crossing the finish line @@ -102,6 +102,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara sol_status = solve(mdl.mdl) sol_u = getvalue(mdl.u_Ol) sol_z = getvalue(mdl.z_Ol) + println("Predicting until z = $(sol_z[end,1])") # COST PRINTS: ******************************************************** # println("coeff: $(getvalue(mdl.coeff))") @@ -125,11 +126,11 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara #println(getvalue(mdl.u_Ol)) #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(mdl.u_Ol),getvalue(mdl.z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) mpcSol.a_x = sol_u[1,1] - mpcSol.d_f = sol_u[2,1] + mpcSol.d_f = sol_u[1,2] mpcSol.u = sol_u mpcSol.z = sol_z - mpcSol.cost = zeros(6) - #mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] + #mpcSol.cost = zeros(6) + mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(mdl.z_Ol[1,N+1])) From fd3c33ba2305153f4753e966e683da8bb6736f59 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 21 Sep 2016 16:05:55 -0700 Subject: [PATCH 029/183] Added more tools to analyze predictive control. Currently no perfect path following, debugging process ongoing. --- eval_sim/create_track.py | 41 ++-- eval_sim/eval_sim.jl | 212 +++++++++++++++--- workspace/src/barc/msg/pos_info.msg | 3 + workspace/src/barc/src/LMPC_node.jl | 62 +++-- .../src/barc/src/Localization_helpers.py | 47 ++-- workspace/src/barc/src/Localization_node.py | 7 +- workspace/src/barc/src/barc_simulator.jl | 6 +- workspace/src/barc/src/helper/functions.jl | 45 ++-- .../src/barc/src/helper/solveMpcProblem.jl | 10 +- 9 files changed, 324 insertions(+), 109 deletions(-) diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index 905a83f0..3d3951e4 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -23,30 +23,33 @@ def add_curve(theta, length, angle): theta = array([0]) -theta = add_curve(theta,10,0) -theta = add_curve(theta,50,-2*pi/3) -#theta = add_curve(theta,30,0) -theta = add_curve(theta,70,pi) -theta = add_curve(theta,60,-5*pi/6) -theta = add_curve(theta,10,0) -theta = add_curve(theta,30,-pi/2) -theta = add_curve(theta,40,0) -theta = add_curve(theta,20,-pi/4) -theta = add_curve(theta,20,pi/4) -theta = add_curve(theta,50,-pi/2) -theta = add_curve(theta,22,0) -theta = add_curve(theta,30,-pi/2) -theta = add_curve(theta,14,0) -#theta = add_curve(theta,7,0) -#theta = add_curve(theta,30,3*pi/4) -#theta = add_curve(theta,10,-1*pi/4) -#theta = add_curve(theta,20,-pi/2) -#theta = add_curve(theta,20,0) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,50,-2*pi/3) +# #theta = add_curve(theta,30,0) +# theta = add_curve(theta,70,pi) +# theta = add_curve(theta,60,-5*pi/6) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,30,-pi/2) +# theta = add_curve(theta,40,0) +# theta = add_curve(theta,20,-pi/4) +# theta = add_curve(theta,20,pi/4) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,22,0) +# theta = add_curve(theta,30,-pi/2) +# theta = add_curve(theta,14,0) + +theta = add_curve(theta,50,0) +theta = add_curve(theta,100,-pi) +theta = add_curve(theta,100,0) +theta = add_curve(theta,100,-pi) +theta = add_curve(theta,49,0) for i in range(0,size(theta)): x = hstack((x, x[-1] + cos(theta[i])*ds)) y = hstack((y, y[-1] + sin(theta[i])*ds)) +print x +print y plt.plot(x,y,'-o') plt.axis('equal') plt.grid() diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index fec19617..8f2beb53 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -13,6 +13,24 @@ const log_path = "$(homedir())/simulations/output.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" const log_path_profile = "$(homedir())/simulations/profile.jlprof" + +function simModel(z,u,dt,l_A,l_B) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = z + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + + return zNext +end + function eval_sim() d = load(log_path) @@ -24,7 +42,6 @@ function eval_sim() track = create_track(0.2) figure() - hold(1) plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) @@ -44,29 +61,78 @@ function eval_sim() end function eval_LMPC() - d = load(log_path_LMPC) - oldTraj = d["oldTraj"] - t = d["t"] - state = d["state"] - sol_z = d["sol_z"] - sol_u = d["sol_u"] - cost = d["cost"] - curv = d["curv"] + d_sim = load(log_path) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + pred_z = d_lmpc["pred_z"] + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + est = d_sim["estimate"] + imu_meas = d_sim["imu_meas"] + gps_meas = d_sim["gps_meas"] + z = d_sim["z"] + cmd_log = d_sim["cmd_log"] + + figure() + plot(z.t,z.z[:,3],t,x_est[:,3]) + grid() + + track = create_track(0.2) + figure() + hold(1) + plot(x_est[:,1],x_est[:,2],"-o") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + for i=1:size(x_est,1) + #dir = [cos(x_est[i,3]) sin(x_est[i,3])] + #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] + #lin = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir] + #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] + #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") + end + for i=1:size(x_est,1) + if i%4==0 + z_pred = zeros(11,4) + z_pred[1,:] = x_est[i,:] + for j=2:11 + z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-o") + end + end + + for i=1:size(x_est,1) + s = 0:.1:3.5 + ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + x = ss*coeffX[i,:]' + y = ss*coeffY[i,:]' + plot(x,y) + end + grid() + + figure() plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") legend(["e_y","e_psi","v"]) grid(1) figure() ax1=subplot(211) - plot(t,state) + plot(1:size(state,1),state,1:size(state,1),pred_z) legend(["s","e_y","e_psi","v"]) grid(1) #figure() subplot(212,sharex = ax1) - plot(t,cost) + plot(1:size(cost,1),cost) grid(1) legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) figure() - plot(t,curv) + plot(1:size(curv,1),curv) legend(["1","2","3","4","5","6","7","8","9"]) end @@ -76,6 +142,94 @@ function eval_oldTraj(i) plot(oldTraj[:,1,1,i],oldTraj[:,2:4,1,i],"-o",oldTraj[:,1,2,i],oldTraj[:,2:4,2,i],"-*") end +function eval_LMPC_coeff(k) + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + s_start = d["s_start"] + + s = sol_z[:,1,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + grid() + title("Position = $(s_start[k] + s[1]), k = $k") + xlabel("s") + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(313) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + grid() + xlabel("s") + ylabel("v") +end + +function anim_LMPC(k1,k2) + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + s_start = d["s_start"] + state = d["state"] + + N = size(sol_z,1)-1 + + for k=k1:k2 + s = sol_z[1:10,1,k] + subplot(211) + plot(s,sol_u[:,1,k],"-o") + ylim([-1,2]) + xlim([0,2]) + grid() + subplot(212) + plot(s,sol_u[:,2,k],"-o") + ylim([-0.5,0.5]) + xlim([0,2]) + grid() + sleep(0.1) + end + + figure() + hold(0) + for k=k1:k2 + s_state = s_start[k:k+N] + state[k:k+N,1] - s_start[k] + s = sol_z[:,1,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,2,k],"-o",s_state,state[k:k+N,2],"-*",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + grid() + title("Position = $(s_start[k] + s[1]), k = $k") + xlabel("s") + xlim([0,3]) + ylim([-0.2,0.2]) + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,3,k],"-o",s_state,state[k:k+N,3],"-*",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + grid() + xlabel("s") + ylabel("e_Psi") + xlim([0,3]) + ylim([-0.2,0.2]) + subplot(313) + plot(s,sol_z[:,4,k],"-o",s_state,state[k:k+N,4],"-*",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + grid() + xlabel("s") + ylabel("v") + xlim([0,3]) + ylim([0,2]) + sleep(0.1) + end +end + function anim_MPC(z) figure() hold(0) @@ -122,19 +276,27 @@ function create_track(w) theta = [0.0] - add_curve(theta,10,0.0) - add_curve(theta,50,-2*pi/3) - add_curve(theta,70,pi) - add_curve(theta,60,-5*pi/6) - add_curve(theta,10,0.0) - add_curve(theta,30,-pi/2) - add_curve(theta,40,0.0) - add_curve(theta,20,-pi/4) - add_curve(theta,20,pi/4) - add_curve(theta,50,-pi/2) - add_curve(theta,22,0.0) - add_curve(theta,30,-pi/2) - add_curve(theta,14,0.0) + # SOPHISTICATED TRACK + # add_curve(theta,10,0.0) + # add_curve(theta,50,-2*pi/3) + # add_curve(theta,70,pi) + # add_curve(theta,60,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,30,-pi/2) + # add_curve(theta,40,0.0) + # add_curve(theta,20,-pi/4) + # add_curve(theta,20,pi/4) + # add_curve(theta,50,-pi/2) + # add_curve(theta,22,0.0) + # add_curve(theta,30,-pi/2) + # add_curve(theta,14,0.0) + + # SIMPLE track + add_curve(theta,50,0) + add_curve(theta,100,-pi) + add_curve(theta,100,0) + add_curve(theta,100,-pi) + add_curve(theta,49,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 03a94251..ada342f5 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -5,6 +5,9 @@ float64 ey float64 epsi float64 v float64 s_start +float64 x +float64 y +float64 psi float64[] coeffX float64[] coeffY float64[] coeffTheta diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index de1ce00f..8c15ef55 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -18,11 +18,15 @@ include("helper/solveMpcProblem.jl") include("helper/computeCostLap.jl") include("helper/functions.jl") -function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1}) # update current position and track data +function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, + coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data # update mpc initial condition z_est[:] = [msg.s,msg.ey,msg.epsi,msg.v] # use z_est as pointer s_start_update[1] = msg.s_start coeffCurvature_update[:] = msg.coeffCurvature + x_est[:] = [msg.x,msg.y,msg.psi,msg.v] + coeffX[:] = msg.coeffX + coeffY[:] = msg.coeffY end function main() @@ -34,6 +38,7 @@ function main() log_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps log_t = zeros(10000,1) log_state = zeros(10000,4) + log_pred_z = zeros(10000,4) log_cost = zeros(10000,6) # Define and initialize variables @@ -47,23 +52,32 @@ function main() mpcParams = MpcParams() z_est = zeros(4) + x_est = zeros(4) + coeffX = zeros(7) + coeffY = zeros(7) s_start_update = [0.0] cmd = ECU(0.0,0.0) InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) - log_sol_z = zeros(mpcParams.N+1,4,10000) - log_sol_u = zeros(mpcParams.N,2,10000) + log_coeff_Cost = zeros(mpcCoeff.order+1,2,10000) + log_coeff_Const = zeros(mpcCoeff.order+1,2,3,10000) + log_sol_z = zeros(mpcParams.N+1,4,10000) + log_sol_u = zeros(mpcParams.N,2,10000) coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) + log_s_start = zeros(10000) + log_state_x = zeros(10000,4) + log_coeffX = zeros(10000,7) + log_coeffY = zeros(10000,7) # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) pub = Publisher("ecu", ECU, queue_size=10) pub2 = Publisher("logging", Logging, queue_size=10) # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) + s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=10) println("Finished initialization.") # Lap parameters @@ -100,8 +114,8 @@ function main() if z_est[1] > 0 # check if data has been received (s > 0) # publish command from last calculation - cmd = ECU(mpcSol.a_x, mpcSol.d_f) - publish(pub, cmd) + # cmd = ECU(mpcSol.a_x, mpcSol.d_f) + # publish(pub, cmd) # ============================= Initialize iteration parameters ============================= lapStatus.currentIt += 1 # count iteration @@ -113,7 +127,7 @@ function main() trackCoeff.coeffCurvature = coeffCurvature_update # Simulate model for next input - simModel(pred_z,zCurr[i,:]',uCurr[i,:]',modelParams.dt,modelParams.l_A,modelParams.l_B,trackCoeff) + simModel(pred_z,zCurr[i,:]',uCurr[i,:]',modelParams,trackCoeff) # this simulation returns the predicted state at when the next command is going to be sent. This predicted state is used for # the MPC control input calculation. @@ -161,17 +175,14 @@ function main() println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") # Find coefficients for cost and constraints - tic() if lapStatus.currentLap > 1 coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) end - tt = toq() - println("Finished coefficients, t = $tt s") # Solve the MPC problem tic() - #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') tt = toq() # Write in current input information @@ -185,14 +196,25 @@ function main() # Logging k = k + 1 # counter - log_state[k,:] = z_est - log_t[k] = time() - t0 - log_sol_z[:,:,k] = mpcSol.z - log_sol_u[:,:,k] = mpcSol.u - log_cost[k,:] = mpcSol.cost - log_curv[k,:] = trackCoeff.coeffCurvature + log_state[k,:] = z_est + log_t[k] = time() + log_sol_z[:,:,k] = mpcSol.z + log_sol_u[:,:,k] = mpcSol.u + log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost + log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst + log_cost[k,:] = mpcSol.cost + log_curv[k,:] = trackCoeff.coeffCurvature + log_s_start[k] = posInfo.s_start + log_pred_z[k,:] = pred_z + log_state_x[k,:] = x_est + log_coeffX[k,:] = coeffX + log_coeffY[k,:] = coeffY println("pred_z = $pred_z") + # publish command from last calculation + cmd = ECU(mpcSol.a_x, mpcSol.d_f) + publish(pub, cmd) + else println("No estimation data received!") end @@ -201,7 +223,9 @@ function main() # Save simulation data to file log_path = "$(homedir())/simulations/output_LMPC.jld" - save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k],"cost",log_cost[1:k,:],"curv",log_curv[1:k,:]) + save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], + "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, + "s_start",log_s_start[1:k],"pred_z",log_pred_z[1:k,:],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:]) println("Exiting LMPC node. Saved data.") end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index d0e5de63..3f19a35d 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -30,15 +30,15 @@ class Localization: psi = 0 # current orientation nodes = 0 # all nodes are saved in a matrix N_nodes_poly_back = 10 # number of nodes behind current position - N_nodes_poly_front = 40 # number of nodes in front + N_nodes_poly_front = 30 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total OrderXY = 6 # order of x-y-polynomial interpolation - OrderThetaCurv = 6 # order of theta interpolation + OrderThetaCurv = 3 # order of theta interpolation closed = True # open or closed trajectory? - coeffX = 0 - coeffY = 0 + coeffX = zeros(11) + coeffY = zeros(11) coeffTheta = 0 coeffCurvature = 0 @@ -47,6 +47,8 @@ class Localization: ey = 0 # lateral distance to path epsi = 0 # error in psi (between current psi and trajectory tangent) v = 0 # current velocity (not necessary for these calculations but for MPC) + x = 0 + y = 0 def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to make sure that all points are equidistant! ang = linspace(0,2*pi-2*pi/n,n) @@ -103,19 +105,26 @@ def create_track(self): ds = 0.06 theta = array([0]) - theta = add_curve(theta,10,0) - theta = add_curve(theta,50,-2*pi/3) - theta = add_curve(theta,70,pi) - theta = add_curve(theta,60,-5*pi/6) - theta = add_curve(theta,10,0) - theta = add_curve(theta,30,-pi/2) - theta = add_curve(theta,40,0) - theta = add_curve(theta,20,-pi/4) - theta = add_curve(theta,20,pi/4) - theta = add_curve(theta,50,-pi/2) - theta = add_curve(theta,22,0) - theta = add_curve(theta,30,-pi/2) - theta = add_curve(theta,14,0) + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,50,-2*pi/3) + # theta = add_curve(theta,70,pi) + # theta = add_curve(theta,60,-5*pi/6) + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,30,-pi/2) + # theta = add_curve(theta,40,0) + # theta = add_curve(theta,20,-pi/4) + # theta = add_curve(theta,20,pi/4) + # theta = add_curve(theta,50,-pi/2) + # theta = add_curve(theta,22,0) + # theta = add_curve(theta,30,-pi/2) + # theta = add_curve(theta,14,0) + + # SIMPLE RACETRACK: + theta = add_curve(theta,50,0) + theta = add_curve(theta,100,-pi) + theta = add_curve(theta,100,0) + theta = add_curve(theta,100,-pi) + theta = add_curve(theta,49,0) for i in range(0,size(theta)): x = hstack((x, x[-1] + cos(theta[i])*ds)) @@ -202,6 +211,8 @@ def set_pos(self,x,y,psi,v): self.pos = array([x,y]) self.psi = psi self.v = v + self.x = x + self.y = y def find_s(self): dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of current position to all nodes @@ -275,6 +286,8 @@ def find_s(self): b_curvature_vector[j] = (dX*ddY-dY*ddX)/(dX**2+dY**2)**1.5 # this calculates the curvature values for all points in the interp. interval # these values are going to be approximated by a polynomial! + # print b_curvature_vector[10:20] + # print b_curvature_vector[10:15] # calculate coefficients for Theta and curvature coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index 72f7c873..ba539f47 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -11,7 +11,7 @@ from numpy import * from Localization_helpers import Localization from barc.msg import Z_KinBkMdl, pos_info - +import matplotlib.pyplot as plt l = 0 epsi_prev = 0 @@ -46,6 +46,9 @@ def localization_node(): #l.create_track2() # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) l.prepare_trajectory(0.06) + #plt.plot(l.nodes[0,:],l.nodes[1,:],'-o') + #print l.nodes + #plt.show() # set node rate loop_rate = 50 @@ -56,7 +59,7 @@ def localization_node(): while not rospy.is_shutdown(): # publish information - state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.coeffX,l.coeffY,l.coeffTheta,l.coeffCurvature) ) + state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.psi,l.coeffX,l.coeffY,l.coeffTheta,l.coeffCurvature) ) # wait rate.sleep() diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 82e3a9e8..02fb8fef 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -49,6 +49,8 @@ est_meas = Measurements{Float32}(0,zeros(Float32,buffersize,1),zeros(Float32,buf cmd_log = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,4)) +z_real.t[1] = time() + function simModel(z,u,dt,l_A,l_B) # kinematic bicycle model @@ -102,7 +104,7 @@ function main() s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) z_current = zeros(60000,4) - z_current[1,:] = [0.1 0 0 0] + z_current[1,:] = [0.2 0 0 0] dt = 0.01 loop_rate = Rate(1/dt) @@ -127,7 +129,7 @@ function main() println("Publishing sensor information. Simulator running.") while ! is_shutdown() - t = time() - t0 + t = time() # update current state with a new row vector z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 5d9a4e9f..5f992970 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -34,28 +34,28 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) mpcParams.N = 10 mpcParams.nz = 4 - mpcParams.Q = [0.0,10.0,1.0,1.0] # put weights on ey, epsi and v - mpcParams.R = 0.1*[1.0,1.0] # put weights on a and d_f - mpcParams.QderivZ = 0.0*[1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1*[1,10] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 0.5 + mpcParams.Q = [0.0,100.0,100.0,10.0] # put weights on ey, epsi and v + mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f + mpcParams.QderivZ = 0*[0,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 0*[1,1] # cost matrix for derivative cost of inputs + mpcParams.vPathFollowing = 1.0 - trackCoeff.nPolyCurvature = 6 # 4th order polynomial for curvature approximation + trackCoeff.nPolyCurvature = 3 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.width = 0.4 # width of the track (0.5m) - modelParams.u_lb = [-1.0 -pi/6]' * ones(1,mpcParams.N) # lower bounds on steering - modelParams.u_ub = [2.0 pi/6]' * ones(1,mpcParams.N) # upper bounds - modelParams.z_lb = [-Inf -trackCoeff.width/2 -Inf -Inf]' * ones(1,mpcParams.N+1) # lower bounds on states - modelParams.z_ub = [Inf trackCoeff.width/2 Inf Inf]' * ones(1,mpcParams.N+1) # upper bounds + modelParams.u_lb = ones(mpcParams.N,1) * [-1.0 -pi/6] # lower bounds on steering + modelParams.u_ub = ones(mpcParams.N,1) * [2.0 pi/6] # upper bounds + modelParams.z_lb = ones(mpcParams.N+1,1)*[-Inf -trackCoeff.width/2 -Inf -Inf] # lower bounds on states + modelParams.z_ub = ones(mpcParams.N+1,1)*[Inf trackCoeff.width/2 Inf Inf] # upper bounds #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) modelParams.c0 = [1, 0.63, 1, 0] # BARC-specific parameters (measured) modelParams.l_A = 0.125 modelParams.l_B = 0.125 modelParams.dt = 0.1 - posInfo.s_start = 0 - posInfo.s_target = 25.62#29.491949#13.20#10.281192 + posInfo.s_start = 0.0 + posInfo.s_target = 24.0#25.62#29.491949#13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) @@ -84,7 +84,7 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation - m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) + m.mdl = Model(solver = IpoptSolver(print_level=0))#,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) @variable( m.mdl, m.z_Ol[1:(N+1),1:4]) # z = s, ey, epsi, v @variable( m.mdl, m.u_Ol[1:N,1:2]) @@ -92,14 +92,14 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara for i=1:2 # I don't know why but somehow the short method returns errors sometimes for j=1:N - setlowerbound(m.u_Ol[j,i], modelParams.u_lb[i,j]) - setupperbound(m.u_Ol[j,i], modelParams.u_ub[i,j]) + setlowerbound(m.u_Ol[j,i], modelParams.u_lb[j,i]) + setupperbound(m.u_Ol[j,i], modelParams.u_ub[j,i]) end end # for i=1:4 # for j=1:N+1 - # setlowerbound(m.z_Ol[i,j], modelParams.z_lb[i,j]) - # setupperbound(m.z_Ol[i,j], modelParams.z_ub[i,j]) + # setlowerbound(m.z_Ol[j,i], modelParams.z_lb[j,i]) + # setupperbound(m.z_Ol[j,i], modelParams.z_ub[j,i]) # end # end #@variable( m.mdl, 1 >= m.ParInt >= 0 ) @@ -111,7 +111,7 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara #@NLexpression(m.mdl, m.c[i = 1:N], m.coeff[1]*m.z_Ol[1,i]^4+m.coeff[2]*m.z_Ol[1,i]^3+m.coeff[3]*m.z_Ol[1,i]^2+m.coeff[4]*m.z_Ol[1,i]+m.coeff[5]) @NLexpression(m.mdl, m.c[i = 1:N], sum{m.coeff[j]*m.z_Ol[i,1]^(n_poly_curv-j+1),j=1:n_poly_curv} + m.coeff[n_poly_curv+1]) - @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * tan( m.u_Ol[i,2] ) ) ) + @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * ( m.u_Ol[i,2] ) ) ) @NLexpression(m.mdl, m.dsdt[i = 1:N], m.z_Ol[i,4]*cos(m.z_Ol[i,3]+m.bta[i])/(1-m.z_Ol[i,2]*m.c[i])) # System dynamics @@ -119,22 +119,25 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara @NLconstraint(m.mdl, m.z_Ol[i+1,1] == m.z_Ol[i,1] + dt*m.dsdt[i] ) # s @NLconstraint(m.mdl, m.z_Ol[i+1,2] == m.z_Ol[i,2] + dt*m.z_Ol[i,4]*sin(m.z_Ol[i,3]+m.bta[i]) ) # ey @NLconstraint(m.mdl, m.z_Ol[i+1,3] == m.z_Ol[i,3] + dt*(m.z_Ol[i,4]/L_a*sin(m.bta[i])-m.dsdt[i]*m.c[i]) ) # epsi - @NLconstraint(m.mdl, m.z_Ol[i+1,4] == m.z_Ol[i,4] + dt*(c0[1]*m.u_Ol[i,1] - c0[2]*abs(m.z_Ol[i,4]) * m.z_Ol[i,4])) # v + @NLconstraint(m.mdl, m.z_Ol[i+1,4] == m.z_Ol[i,4] + dt*(m.u_Ol[i,1] - 0.0*abs(m.z_Ol[i,4]) * m.z_Ol[i,4])) # v end end -function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},dt::Float64,l_A::Float64,l_B::Float64,trackCoeff::TrackCoeff) +function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},modelParams::ModelParams,trackCoeff::TrackCoeff) # kinematic bicycle model # u[1] = acceleration # u[2] = steering angle - + dt = modelParams.dt + l_A = modelParams.l_A + l_B = modelParams.l_B s = z[1] c = 0 for i=trackCoeff.nPolyCurvature:-1:0 c += s^i * trackCoeff.coeffCurvature[trackCoeff.nPolyCurvature+1-i] end + # println("Sim Model, c = $c") #c = ([s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s 1] * coeff'')[1] bta = atan(l_A/(l_A+l_B)*tan(u[2])) dsdt = z[4] *cos(z[3]+bta)/(1-z[2]*c) diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 983ad3cd..e5e68b4c 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -53,8 +53,8 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Derivative cost # --------------------------------- - @NLexpression(mdl.mdl, derivCost, 0*sum{QderivZ[j]*((zCurr[j]-mdl.z_Ol[1,j])^2+sum{(mdl.z_Ol[i,j]-mdl.z_Ol[i+1,j])^2,i=1:N}),j=1:4} + - sum{QderivU[j]*((uCurr[j]-mdl.u_Ol[1,j])^2+sum{(mdl.u_Ol[i,j]-mdl.u_Ol[i+1,j])^2,i=1:N-1}),j=1:2}) + @NLexpression(mdl.mdl, derivCost, sum{QderivZ[j]*((zCurr[j]-mdl.z_Ol[1,j])^2+sum{(mdl.z_Ol[i,j]-mdl.z_Ol[i+1,j])^2,i=1:N}),j=1:4} + + sum{QderivU[j]*((uCurr[j]-mdl.u_Ol[1,j])^2+sum{(mdl.u_Ol[i,j]-mdl.u_Ol[i+1,j])^2,i=1:N-1}),j=1:2}) # Lane cost # --------------------------------- @@ -77,10 +77,10 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # --------------------------------- # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line. if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, costZTerm, 5*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ + @NLexpression(mdl.mdl, costZTerm, 1*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1})) elseif lapStatus.currentLap == 2 # if we're in the second second lap - @NLexpression(mdl.mdl, costZTerm, 5*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl.mdl, costZTerm, 1*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}) end # State cost @@ -103,6 +103,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara sol_u = getvalue(mdl.u_Ol) sol_z = getvalue(mdl.z_Ol) println("Predicting until z = $(sol_z[end,1])") + println("curvature = $(getvalue(mdl.c))") # COST PRINTS: ******************************************************** # println("coeff: $(getvalue(mdl.coeff))") @@ -129,6 +130,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara mpcSol.d_f = sol_u[1,2] mpcSol.u = sol_u mpcSol.z = sol_z + mpcSol.solverStatus = sol_status #mpcSol.cost = zeros(6) mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging From a7fe1f1d1890c1619f13e0c8af1bba58716c42b6 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 22 Sep 2016 21:34:45 -0700 Subject: [PATCH 030/183] Working version. LMPC learns and is stable (if the parameters are chosen correctly and not too agressive) --- eval_sim/eval_sim.jl | 55 +++++++------- workspace/src/barc/src/LMPC_node.jl | 6 +- .../src/barc/src/Localization_helpers.py | 71 +++++++++++-------- workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 19 +++-- workspace/src/barc/src/helper/classes.jl | 3 +- workspace/src/barc/src/helper/functions.jl | 23 +++--- .../src/barc/src/helper/solveMpcProblem.jl | 13 ++-- 8 files changed, 108 insertions(+), 84 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 8f2beb53..bf18474e 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -40,6 +40,7 @@ function eval_sim() z = d["z"] cmd_log = d["cmd_log"] + t0 = est.t[1] track = create_track(0.2) figure() plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") @@ -47,11 +48,11 @@ function eval_sim() grid(1) legend(["real state","GPS meas","estimate"]) figure() - plot(z.t,z.z[:,3],imu_meas.t,imu_meas.z,est.t,est.z[:,3]) + plot(z.t-t0,z.z[:,3],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) grid(1) legend(["Real psi","psi meas","estimate"]) figure() - plot(z.t,z.z[:,4]) + plot(z.t-t0,z.z[:,4]) grid() legend(["Velocity"]) figure() @@ -75,6 +76,7 @@ function eval_LMPC() x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] + s_start = d_lmpc["s_start"] est = d_sim["estimate"] imu_meas = d_sim["imu_meas"] gps_meas = d_sim["gps_meas"] @@ -85,6 +87,7 @@ function eval_LMPC() plot(z.t,z.z[:,3],t,x_est[:,3]) grid() + t0 = t[1] track = create_track(0.2) figure() hold(1) @@ -98,19 +101,19 @@ function eval_LMPC() #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") end for i=1:size(x_est,1) - if i%4==0 + if i%1==0 z_pred = zeros(11,4) z_pred[1,:] = x_est[i,:] for j=2:11 z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) end - plot(z_pred[:,1],z_pred[:,2],"-o") + plot(z_pred[:,1],z_pred[:,2],"-*") end end for i=1:size(x_est,1) - s = 0:.1:3.5 - ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + s = 0.4:.1:2.5 + ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] x = ss*coeffX[i,:]' y = ss*coeffY[i,:]' plot(x,y) @@ -123,12 +126,12 @@ function eval_LMPC() grid(1) figure() ax1=subplot(211) - plot(1:size(state,1),state,1:size(state,1),pred_z) + plot(t-t0,state,z.t-t0,z.z[:,1:2]) legend(["s","e_y","e_psi","v"]) grid(1) #figure() subplot(212,sharex = ax1) - plot(1:size(cost,1),cost) + plot(t-t0,cost) grid(1) legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) figure() @@ -277,26 +280,26 @@ function create_track(w) theta = [0.0] # SOPHISTICATED TRACK - # add_curve(theta,10,0.0) - # add_curve(theta,50,-2*pi/3) - # add_curve(theta,70,pi) - # add_curve(theta,60,-5*pi/6) - # add_curve(theta,10,0.0) - # add_curve(theta,30,-pi/2) - # add_curve(theta,40,0.0) - # add_curve(theta,20,-pi/4) - # add_curve(theta,20,pi/4) - # add_curve(theta,50,-pi/2) - # add_curve(theta,22,0.0) - # add_curve(theta,30,-pi/2) - # add_curve(theta,14,0.0) + add_curve(theta,60,0.0) + add_curve(theta,50,-2*pi/3) + add_curve(theta,70,pi) + add_curve(theta,60,-5*pi/6) + add_curve(theta,10,0.0) + add_curve(theta,30,-pi/2) + add_curve(theta,90,0.0) + add_curve(theta,20,-pi/4) + add_curve(theta,20,pi/4) + add_curve(theta,50,-pi/2) + add_curve(theta,22,0.0) + add_curve(theta,30,-pi/2) + add_curve(theta,14,0.0) # SIMPLE track - add_curve(theta,50,0) - add_curve(theta,100,-pi) - add_curve(theta,100,0) - add_curve(theta,100,-pi) - add_curve(theta,49,0) + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 8c15ef55..69fa40c1 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -134,6 +134,7 @@ function main() # ======================================= Lap trigger ======================================= # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! + if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... # ... then select and save data println("Saving data") @@ -144,6 +145,7 @@ function main() uCurr[1,:] = uCurr[i+1,:] # ... and input i = 1 lapStatus.currentLap += 1 # start next lap + #lapStatus.currentLap = 1 lapStatus.currentIt = 1 # reset current iteration switchLap = false @@ -168,7 +170,7 @@ function main() println("======================================== NEW ITERATION # $i ========================================") println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") println("State Nr. $i = $z_est") - #println("Coeff Curvature = $(trackCoeff.coeffCurvature)") + println("Coeff Curvature = $(trackCoeff.coeffCurvature)") println("posInfo = $posInfo") println("s = $(posInfo.s)") println("s_start = $(posInfo.s_start)") @@ -209,7 +211,7 @@ function main() log_state_x[k,:] = x_est log_coeffX[k,:] = coeffX log_coeffY[k,:] = coeffY - println("pred_z = $pred_z") + #println("pred_z = $pred_z") # publish command from last calculation cmd = ECU(mpcSol.a_x, mpcSol.d_f) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 3f19a35d..d5d8eef4 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -34,7 +34,7 @@ class Localization: ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total OrderXY = 6 # order of x-y-polynomial interpolation - OrderThetaCurv = 3 # order of theta interpolation + OrderThetaCurv = 5 # order of theta interpolation closed = True # open or closed trajectory? coeffX = zeros(11) @@ -105,26 +105,29 @@ def create_track(self): ds = 0.06 theta = array([0]) - # theta = add_curve(theta,10,0) - # theta = add_curve(theta,50,-2*pi/3) - # theta = add_curve(theta,70,pi) - # theta = add_curve(theta,60,-5*pi/6) - # theta = add_curve(theta,10,0) - # theta = add_curve(theta,30,-pi/2) - # theta = add_curve(theta,40,0) - # theta = add_curve(theta,20,-pi/4) - # theta = add_curve(theta,20,pi/4) - # theta = add_curve(theta,50,-pi/2) - # theta = add_curve(theta,22,0) - # theta = add_curve(theta,30,-pi/2) - # theta = add_curve(theta,14,0) - - # SIMPLE RACETRACK: - theta = add_curve(theta,50,0) - theta = add_curve(theta,100,-pi) - theta = add_curve(theta,100,0) - theta = add_curve(theta,100,-pi) - theta = add_curve(theta,49,0) + # Sophisticated racetrack: length = 25.62m + theta = add_curve(theta,60,0) + theta = add_curve(theta,50,-2*pi/3) + theta = add_curve(theta,70,pi) + theta = add_curve(theta,60,-5*pi/6) + theta = add_curve(theta,10,0) + theta = add_curve(theta,30,-pi/2) + theta = add_curve(theta,90,0) + theta = add_curve(theta,20,-pi/4) + theta = add_curve(theta,20,pi/4) + theta = add_curve(theta,50,-pi/2) + theta = add_curve(theta,22,0) + theta = add_curve(theta,30,-pi/2) + theta = add_curve(theta,14,0) + + # SIMPLE RACETRACK (smooth curves): length = 24.0m + # theta = add_curve(theta,50,0) + # theta = add_curve(theta,100,-pi) + # theta = add_curve(theta,100,0) + # theta = add_curve(theta,100,-pi) + # theta = add_curve(theta,49,0) + + # SIMPLER RACETRACK (half circles as curves): for i in range(0,size(theta)): x = hstack((x, x[-1] + cos(theta[i])*ds)) @@ -182,9 +185,11 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e rem = lambda x:mod(length,x) # function to calculate optimal step size (for evenly distributed steps) sol = scipy.optimize.fmin(func=rem,x0=dsn,args=(),xtol=0.0000001,ftol=0.0000001) dsn = sol[0] + #dsn = 0.06 #sn = arange(0,length,dsn) # new steps #n = size(sn,0) n = floor(length/dsn) + #n = 500 #print(n) sn = arange(0,n*dsn,dsn) #print(sn) @@ -203,8 +208,9 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e print "Finished track optimization." print "Track length = %fm, ds = %fm"%(length,dsn) print "Approximated length = %fm"%(self.nPoints*dsn) - #print(sn) - #print(self.nodes) + print "# Nodes: %f"%n + print(sn) + print(self.nodes) def set_pos(self,x,y,psi,v): @@ -250,6 +256,8 @@ def find_s(self): # Create Matrix for interpolation # x-y-Matrix + # The x-y-matrix is just filled with s^n values (n from 0 to the polynomial degree) for s values between s = 0 and s = nPoints*ds + # with nPoints = number of points that approximate the polynomial Matrix = zeros([self.nPoints,self.OrderXY+1]) for i in range(0,self.nPoints): for k in range(0,self.OrderXY+1): @@ -300,10 +308,15 @@ def find_s(self): # Calculate s discretization = 0.001 # discretization to calculate s - s_idx_start = max(0,idx_min-1) # where the discretization starts - if self.closed and s_idx_start-idx_start<0: - s_idx_start = s_idx_start + n - j = arange((s_idx_start-idx_start)*self.ds,(s_idx_start-idx_start+2)*self.ds,discretization) + + j = arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) + #print "idx_min = %f"%idx_min + #print "s_idx_start = %f"%s_idx_start + #print "idx_start = %f"%idx_start + #print "idx_end = %f"%idx_end + #print "n = %f"%n + #print j + #print("s_discretization:") #print(j) x_eval = polyval(self.coeffX,j) @@ -312,6 +325,7 @@ def find_s(self): idx_s_min = argmin(dist_eval) s = j[idx_s_min] # s = minimum distance to points between idx_min-1 and idx_min+1 eyabs = amin(dist_eval) # absolute distance to curve + #print dist_eval # Calculate sign of y s0 = s - discretization @@ -329,6 +343,7 @@ def find_s(self): # Calculate epsi epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle epsi = (epsi+pi)%(2*pi)-pi + #print "epsi = %f"%epsi #if abs(epsi) > pi/2: # if epsi < pi/2: # epsi = epsi + 2*pi @@ -340,7 +355,7 @@ def find_s(self): self.s = s self.coeffTheta = coeffTheta self.coeffCurvature = coeffCurvature - self.s_start = idx_start*self.ds + self.s_start = idx_start*self.ds def __init__(self): diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index ba539f47..d6db90dd 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -41,7 +41,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - #l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) + #l.create_racetrack(3.0,3.0,0.2,array([0.0,-1.5]),0) l.create_track() #l.create_track2() # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 02fb8fef..375c3de9 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -26,7 +26,7 @@ using sensor_msgs.msg using JLD u_current = zeros(2,1) -t0 = 0 + t = 0 @@ -49,7 +49,10 @@ est_meas = Measurements{Float32}(0,zeros(Float32,buffersize,1),zeros(Float32,buf cmd_log = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,4)) -z_real.t[1] = time() +z_real.t[1] = time() +imu_meas.t[1] = time() +est_meas.t[1] = time() +cmd_log.t[1] = time() function simModel(z,u,dt,l_A,l_B) @@ -73,16 +76,15 @@ end function ECU_callback(msg::ECU) - global u_current, t0 + global u_current u_current = [msg.motor, msg.servo] cmd_log.i += 1 - cmd_log.t[cmd_log.i] = time()-t0 + cmd_log.t[cmd_log.i] = time() cmd_log.z[cmd_log.i,:] = u_current' end function est_callback(msg::Z_KinBkMdl) - global t0 - t = time() - t0 + t = time() est_meas.i += 1 est_meas.t[est_meas.i] = t est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] @@ -104,7 +106,7 @@ function main() s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) z_current = zeros(60000,4) - z_current[1,:] = [0.2 0 0 0] + z_current[1,:] = [0.01 0.0 0.0 0.0] dt = 0.01 loop_rate = Rate(1/dt) @@ -124,13 +126,10 @@ function main() imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) - global t0 = time() # Start time of the simulation - println("Publishing sensor information. Simulator running.") while ! is_shutdown() t = time() - # update current state with a new row vector z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' z_real.t[i] = t diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl index c6a6d8e5..336cf637 100644 --- a/workspace/src/barc/src/helper/classes.jl +++ b/workspace/src/barc/src/helper/classes.jl @@ -33,11 +33,12 @@ type MpcParams # parameters for MPC solver nz::Int64 OrderCostCons::Int64 Q::Array{Float64,1} + Q_term::Array{Float64,1} R::Array{Float64,1} vPathFollowing::Float64 QderivZ::Array{Float64,1} QderivU::Array{Float64,1} - MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[]) = new(N,nz,OrderCostCons,Q,R,vPathFollowing) + MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[]) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing) end type PosInfo # current position information diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 5f992970..995fc61c 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -8,7 +8,7 @@ function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{F costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line # Save all data in oldTrajectory: if lapStatus.currentLap == 1 # if it's the first lap - oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything + oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything oldTraj.oldInput[:,:,1] = uCurr_export oldTraj.oldTraj[:,:,2] = zCurr_export oldTraj.oldInput[:,:,2] = uCurr_export @@ -34,18 +34,19 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) mpcParams.N = 10 mpcParams.nz = 4 - mpcParams.Q = [0.0,100.0,100.0,10.0] # put weights on ey, epsi and v - mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f + mpcParams.Q = [0.0,10.0,0.1,1.0] # put weights on ey, epsi and v + mpcParams.Q_term = [0.1,0.1,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) + mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 0*[0,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 0*[1,1] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 1.0 + mpcParams.QderivU = 1*[1,1] # cost matrix for derivative cost of inputs + mpcParams.vPathFollowing = 1.0 # reference speed for first lap of path following - trackCoeff.nPolyCurvature = 3 # 4th order polynomial for curvature approximation + trackCoeff.nPolyCurvature = 5 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.width = 0.4 # width of the track (0.5m) + trackCoeff.width = 0.4 # width of the track (0.5m) modelParams.u_lb = ones(mpcParams.N,1) * [-1.0 -pi/6] # lower bounds on steering - modelParams.u_ub = ones(mpcParams.N,1) * [2.0 pi/6] # upper bounds + modelParams.u_ub = ones(mpcParams.N,1) * [1.2 pi/6] # upper bounds modelParams.z_lb = ones(mpcParams.N+1,1)*[-Inf -trackCoeff.width/2 -Inf -Inf] # lower bounds on states modelParams.z_ub = ones(mpcParams.N+1,1)*[Inf trackCoeff.width/2 Inf Inf] # upper bounds #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) @@ -55,7 +56,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP modelParams.dt = 0.1 posInfo.s_start = 0.0 - posInfo.s_target = 24.0#25.62#29.491949#13.20#10.281192 + posInfo.s_target = 31.62#25.62#29.491949#13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldInput = zeros(buffersize,2,2) @@ -84,7 +85,7 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation - m.mdl = Model(solver = IpoptSolver(print_level=0))#,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) + m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) @variable( m.mdl, m.z_Ol[1:(N+1),1:4]) # z = s, ey, epsi, v @variable( m.mdl, m.u_Ol[1:N,1:2]) @@ -119,7 +120,7 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara @NLconstraint(m.mdl, m.z_Ol[i+1,1] == m.z_Ol[i,1] + dt*m.dsdt[i] ) # s @NLconstraint(m.mdl, m.z_Ol[i+1,2] == m.z_Ol[i,2] + dt*m.z_Ol[i,4]*sin(m.z_Ol[i,3]+m.bta[i]) ) # ey @NLconstraint(m.mdl, m.z_Ol[i+1,3] == m.z_Ol[i,3] + dt*(m.z_Ol[i,4]/L_a*sin(m.bta[i])-m.dsdt[i]*m.c[i]) ) # epsi - @NLconstraint(m.mdl, m.z_Ol[i+1,4] == m.z_Ol[i,4] + dt*(m.u_Ol[i,1] - 0.0*abs(m.z_Ol[i,4]) * m.z_Ol[i,4])) # v + @NLconstraint(m.mdl, m.z_Ol[i+1,4] == m.z_Ol[i,4] + dt*(m.u_Ol[i,1] - 0.63*abs(m.z_Ol[i,4]) * m.z_Ol[i,4])) # v end end diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index e5e68b4c..3e113d38 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -13,6 +13,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} N = mpcParams.N Q = mpcParams.Q + Q_term = mpcParams.Q_term R = mpcParams.R coeffTermCost = mpcCoeff.coeffCost::Array{Float64,2} coeffTermConst = mpcCoeff.coeffConst::Array{Float64,3} @@ -44,6 +45,8 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara setvalue(mdl.z0,zCurr) # Update curvature setvalue(mdl.coeff,coeffCurvature) + println("z0 = $(getvalue(mdl.z0))") + println("coeffCurvature = $(getvalue(mdl.coeff))") @NLexpression(mdl.mdl, costZ, 0) @NLexpression(mdl.mdl, costZTerm, 0) @@ -67,10 +70,10 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, constZTerm, 10*(sum{(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ + @NLexpression(mdl.mdl, constZTerm, 10*(sum{Q_term[j]*(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap - @NLexpression(mdl.mdl, constZTerm, 10*sum{(sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3}) + @NLexpression(mdl.mdl, constZTerm, 10*sum{Q_term[j]*(sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3}) end # Terminal cost @@ -103,7 +106,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara sol_u = getvalue(mdl.u_Ol) sol_z = getvalue(mdl.z_Ol) println("Predicting until z = $(sol_z[end,1])") - println("curvature = $(getvalue(mdl.c))") + #println("curvature = $(getvalue(mdl.c))") # COST PRINTS: ******************************************************** # println("coeff: $(getvalue(mdl.coeff))") @@ -131,8 +134,8 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara mpcSol.u = sol_u mpcSol.z = sol_z mpcSol.solverStatus = sol_status - #mpcSol.cost = zeros(6) - mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] + mpcSol.cost = zeros(6) + #mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging #println(getvalue(costZTerm)) #println(getvalue(mdl.z_Ol[1,N+1])) From 99111104b8450510d4da5c818f45958e0238056a Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 23 Sep 2016 13:37:15 -0700 Subject: [PATCH 031/183] Added precompiling (to speed up code) and evaluation functions (to view curvature). LMPC is working with little noise. --- eval_sim/eval_sim.jl | 99 ++++++++++++------- workspace/src/barc/src/LMPC_node.jl | 50 ++++++---- .../src/barc/src/Localization_helpers.py | 16 +-- workspace/src/barc/src/barc_simulator.jl | 12 +-- .../barc/src/helper/coeffConstraintCost.jl | 58 +++++------ workspace/src/barc/src/helper/functions.jl | 65 ++++++------ .../src/barc/src/helper/solveMpcProblem.jl | 2 +- 7 files changed, 174 insertions(+), 128 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index bf18474e..9f4ecac7 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -46,6 +46,8 @@ function eval_sim() plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) + title("x-y-view") + axis("equal") legend(["real state","GPS meas","estimate"]) figure() plot(z.t-t0,z.z[:,3],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) @@ -56,7 +58,7 @@ function eval_sim() grid() legend(["Velocity"]) figure() - plot(cmd_log.t,cmd_log.z) + plot(cmd_log.t-t0,cmd_log.z) legend(["a","d_f"]) grid() end @@ -72,7 +74,7 @@ function eval_LMPC() sol_u = d_lmpc["sol_u"] cost = d_lmpc["cost"] curv = d_lmpc["curv"] - pred_z = d_lmpc["pred_z"] + f_z = d_lmpc["pred_z"] x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] @@ -83,60 +85,91 @@ function eval_LMPC() z = d_sim["z"] cmd_log = d_sim["cmd_log"] + t0 = t[1] + + c = zeros(size(curv,1),1) + for i=1:size(curv,1) + s = state[i,1] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + end + plot(s_start+state[:,1],c,"-o") + for i=1:2:size(curv,1) + s = sol_z[:,1,i] + c = zeros(size(curv,1),1) + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + plot(s_start[i]+s,c,"-*") + end + title("Curvature over path") + xlabel("Curvilinear abscissa [m]") + ylabel("Curvature") + grid() + figure() - plot(z.t,z.z[:,3],t,x_est[:,3]) + plot(z.t-t0,z.z[:,3],t-t0,x_est[:,3]) + legend(["psi_true","psi_est"]) grid() - t0 = t[1] track = create_track(0.2) figure() hold(1) plot(x_est[:,1],x_est[:,2],"-o") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - for i=1:size(x_est,1) - #dir = [cos(x_est[i,3]) sin(x_est[i,3])] - #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] - #lin = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir] - #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] - #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") - end - for i=1:size(x_est,1) - if i%1==0 - z_pred = zeros(11,4) - z_pred[1,:] = x_est[i,:] - for j=2:11 - z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) - end - plot(z_pred[:,1],z_pred[:,2],"-*") - end - end + axis("equal") + # for i=1:size(x_est,1) + # #dir = [cos(x_est[i,3]) sin(x_est[i,3])] + # #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] + # #lin = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir] + # #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] + # #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") + # end + # for i=1:size(x_est,1) + # if i%4==0 + # z_pred = zeros(11,4) + # z_pred[1,:] = x_est[i,:] + # for j=2:11 + # z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + # end + # plot(z_pred[:,1],z_pred[:,2],"-*") + # end + # end + + # for i=1:size(x_est,1) + # s = 0.4:.1:2.5 + # ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + # x = ss*coeffX[i,:]' + # y = ss*coeffY[i,:]' + # plot(x,y) + # end + grid() - for i=1:size(x_est,1) - s = 0.4:.1:2.5 - ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - x = ss*coeffX[i,:]' - y = ss*coeffY[i,:]' - plot(x,y) - end + figure() + plot(t-t0,state,"-o",t-t0,f_z,"-*") + legend(["s","ey","epsi","v","s_pred","ey_pred","epsi_pred","v_pred"]) grid() figure() plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") legend(["e_y","e_psi","v"]) grid(1) + + figure() + plot(s_start+state[:,1],state[:,[2,4]],"*") + grid() + figure() ax1=subplot(211) plot(t-t0,state,z.t-t0,z.z[:,1:2]) - legend(["s","e_y","e_psi","v"]) + legend(["s","e_y","e_psi","v","x","y"]) grid(1) - #figure() subplot(212,sharex = ax1) plot(t-t0,cost) grid(1) legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) - figure() - plot(1:size(curv,1),curv) - legend(["1","2","3","4","5","6","7","8","9"]) + # figure() + # plot(1:size(curv,1),curv) + # grid() + # title("Polynomial coefficients") + # legend(["1","2","3","4","5","6","7","8","9"]) end function eval_oldTraj(i) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 69fa40c1..a76ae35e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -53,8 +53,8 @@ function main() z_est = zeros(4) x_est = zeros(4) - coeffX = zeros(7) - coeffY = zeros(7) + coeffX = zeros(9) + coeffY = zeros(9) s_start_update = [0.0] cmd = ECU(0.0,0.0) @@ -69,8 +69,8 @@ function main() log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) log_s_start = zeros(10000) log_state_x = zeros(10000,4) - log_coeffX = zeros(10000,7) - log_coeffY = zeros(10000,7) + log_coeffX = zeros(10000,9) + log_coeffY = zeros(10000,9) # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) @@ -104,37 +104,50 @@ function main() # Initial solve: println("Initial solve...") solve(mdl.mdl) + solve(mdl.mdl) println("Finished.") - pred_z = zeros(4,1) + pred_z = zeros(4) - t0 = time() k = 0 + + # Precompile functions + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams) + extendOldTraj(oldTraj,posInfo,zCurr) + lapStatus.currentIt = 100 + saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) + lapStatus.currentIt = 0 + mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) # ... and set them back to zeros + mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) + println("Finished precompiling.") + + # Start node while ! is_shutdown() if z_est[1] > 0 # check if data has been received (s > 0) # publish command from last calculation - # cmd = ECU(mpcSol.a_x, mpcSol.d_f) + # cmd.motor = mpcSol.a_x + # cmd.servo = mpcSol.d_f # publish(pub, cmd) # ============================= Initialize iteration parameters ============================= lapStatus.currentIt += 1 # count iteration i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr[i,:] = z_est # update state information + zCurr[i,:] = z_est # update state information: s, e_y, e_psi, v posInfo.s = z_est[1] # update position info posInfo.s_start = s_start_update[1] trackCoeff.coeffCurvature = coeffCurvature_update # Simulate model for next input - simModel(pred_z,zCurr[i,:]',uCurr[i,:]',modelParams,trackCoeff) + #pred_z = simModel(z_est,uCurr[i,:][:],modelParams,trackCoeff) + # this simulation returns the predicted state at when the next command is going to be sent. This predicted state is used for # the MPC control input calculation. # ======================================= Lap trigger ======================================= # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! - if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... # ... then select and save data println("Saving data") @@ -145,7 +158,6 @@ function main() uCurr[1,:] = uCurr[i+1,:] # ... and input i = 1 lapStatus.currentLap += 1 # start next lap - #lapStatus.currentLap = 1 lapStatus.currentIt = 1 # reset current iteration switchLap = false @@ -163,7 +175,7 @@ function main() # if we are at least in the 2nd lap, concatenate the beginning to the end of the previous track if lapStatus.currentLap > 1 && lapStatus.currentIt == 80 - #extendOldTraj(oldTraj,posInfo,zCurr) + extendOldTraj(oldTraj,posInfo,zCurr) end # ======================================= Calculate input ======================================= @@ -171,14 +183,17 @@ function main() println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") println("State Nr. $i = $z_est") println("Coeff Curvature = $(trackCoeff.coeffCurvature)") - println("posInfo = $posInfo") println("s = $(posInfo.s)") println("s_start = $(posInfo.s_start)") println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") # Find coefficients for cost and constraints + if lapStatus.currentLap > 1 - coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) + tic() + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams) + tt = toq() + println("Finished coefficients, t = $tt s") end # Solve the MPC problem @@ -190,7 +205,7 @@ function main() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] #println("trackCoeff = $trackCoeff") - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i+1,:]), t = $tt s") # ... and publish data zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) @@ -211,10 +226,11 @@ function main() log_state_x[k,:] = x_est log_coeffX[k,:] = coeffX log_coeffY[k,:] = coeffY - #println("pred_z = $pred_z") + println("pred_z = $pred_z") # publish command from last calculation - cmd = ECU(mpcSol.a_x, mpcSol.d_f) + cmd.motor = mpcSol.a_x + cmd.servo = mpcSol.d_f publish(pub, cmd) else diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index d5d8eef4..ecf10c06 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -29,12 +29,12 @@ class Localization: pos = 0 # current position psi = 0 # current orientation nodes = 0 # all nodes are saved in a matrix - N_nodes_poly_back = 10 # number of nodes behind current position - N_nodes_poly_front = 30 # number of nodes in front + N_nodes_poly_back = 20 # number of nodes behind current position + N_nodes_poly_front = 40 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 6 # order of x-y-polynomial interpolation - OrderThetaCurv = 5 # order of theta interpolation + OrderXY = 8 # order of x-y-polynomial interpolation + OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? coeffX = zeros(11) @@ -207,10 +207,10 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e self.n = size(xn) print "Finished track optimization." print "Track length = %fm, ds = %fm"%(length,dsn) - print "Approximated length = %fm"%(self.nPoints*dsn) + print "Approximated length in region of s = [%.3f, %.3f]"%((self.N_nodes_poly_back-5)*dsn,(self.nPoints-5)*dsn) print "# Nodes: %f"%n - print(sn) - print(self.nodes) + #print(sn) + #print(self.nodes) def set_pos(self,x,y,psi,v): @@ -307,7 +307,7 @@ def find_s(self): # Calculate s - discretization = 0.001 # discretization to calculate s + discretization = 0.0001 # discretization to calculate s j = arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) #print "idx_min = %f"%idx_min diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 375c3de9..e7a095b5 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -33,7 +33,7 @@ t = 0 # This type contains measurement data (time, values and a counter) type Measurements{T} i::Int64 # measurement counter - t::Array{T} # time data + t::Array{Float64} # time data z::Array{T} # measurement values end # This function cleans the zeros from the type above once the simulation is finished @@ -45,7 +45,7 @@ end buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) imu_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,1)) -est_meas = Measurements{Float32}(0,zeros(Float32,buffersize,1),zeros(Float32,buffersize,4)) +est_meas = Measurements{Float32}(0,zeros(buffersize,1),zeros(Float32,buffersize,4)) cmd_log = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,4)) @@ -69,7 +69,7 @@ function simModel(z,u,dt,l_A,l_B) zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v # Add process noise (depending on velocity) - zNext = zNext + 0*diagm([0.01*z[4],0.01*z[4],0.001,0.01*z[4]])*randn(4,1) + zNext = zNext + diagm([0.001*z[4],0.001*z[4],0.00,0.001*z[4]])*randn(4,1) return zNext end @@ -106,7 +106,7 @@ function main() s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) z_current = zeros(60000,4) - z_current[1,:] = [0.01 0.0 0.0 0.0] + z_current[1,:] = [0.05 0.0 0.0 0.0] dt = 0.01 loop_rate = Rate(1/dt) @@ -163,8 +163,8 @@ function main() end # GPS measurements - x = round(z_current[i,1]*100 + 0*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 0*randn()*2) + x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 1*randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index e5b36ea5..2ac5eafc 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -1,7 +1,6 @@ # This function evaluates and returns the coefficients for constraints and cost which are used later in the MPC formulation # Inputs are: # oldTraj -> contains information about previous trajectories and Inputs -# lapStatus -> contains information about the current lap # mpcCoeff -> contains information about previous coefficient results # posInfo -> contains information about the car's current position along the track # mpcParams -> contains information about the MPC formulation (e.g. Q, R) @@ -10,7 +9,7 @@ # structure of oldTrajectory: 1st dimension = state number, 2nd dimension = step number (time equiv.), 3rd dimennsion = lap number -function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams) +function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams) # this computes the coefficients for the cost and constraints # Outputs: @@ -18,8 +17,6 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # coeffCost # Read Inputs - lapNumber = lapStatus.currentLap - s_start = posInfo.s_start s = posInfo.s s_target = posInfo.s_target @@ -34,7 +31,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo pLength = mpcCoeff.pLength # interpolation length for polynomials coeffCost = zeros(Order+1,2) # polynomial coefficients for cost - coeffConst = zeros(Order+1,2,3) # nz-1 beacuse no coeff for s + coeffConst = zeros(Order+1,2,3) # nz-1 beacuse no coeff for s # Select the old data oldS = oldTraj.oldTraj[:,1,:]::Array{Float64,3} @@ -47,29 +44,29 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo s_total::Float64 # initialize DistS::Array{Float64} # initialize idx_s::Array{Int64} # initialize - idx_s_target = 0 - dist_to_s_target = 0 - qLength = 0 + idx_s_target = 0 + dist_to_s_target = 0 + qLength = 0 vec_range::Tuple{UnitRange{Int64},UnitRange{Int64}} bS_Vector::Array{Float64} s_forinterpy::Array{Float64} # Compute the total s (current position along track) - s_total = (s_start + s) % s_target + s_total = (s_start + s) % s_target - # Compute the index - DistS = ( s_total - oldS ).^2 + # Compute the index + DistS = ( s_total - oldS ).^2 - idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! + idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! - vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) + vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) - # Create the vectors used for the interpolation - bS_Vector = zeros(pLength+1,2) - for i=1:pLength+1 - bS_Vector[i,1] = oldS[vec_range[1][i]] - bS_Vector[i,2] = oldS[vec_range[2][i]] - end + # Create the vectors used for the interpolation + bS_Vector = zeros(pLength+1,2) + for i=1:pLength+1 + bS_Vector[i,1] = oldS[vec_range[1][i]] + bS_Vector[i,2] = oldS[vec_range[2][i]] + end # bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) # println("************************************** COEFFICIENTS **************************************") @@ -110,17 +107,16 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost for i=1:2 - Qfunction = zeros(N_points,1) - IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position - idx_s_target = find(oldS[:,i].>s_target)[1] - dist_to_s_target = (idx_s_target - IndexBezierS) - # println("dist_to_s_target $i = $dist_to_s_target") - dist_to_s_target = dist_to_s_target + 30 - - bQfunction_Vector = zeros(pLength+1,1) + Qfunction = zeros(N_points,1) + IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position + idx_s_target = find(oldS[:,i].>s_target)[1] + dist_to_s_target = (idx_s_target - IndexBezierS) + dist_to_s_target = dist_to_s_target + 30 + + bQfunction_Vector = zeros(pLength+1,1) # Select the part needed for the interpolation #bQfunction_Vector = Qfunction[IndexBezierS:IndexBezierS+pLength] - qLength = min(dist_to_s_target,pLength+1) + qLength = min(dist_to_s_target,pLength+1) #println(bQfunction_Vector) bQfunction_Vector = zeros(pLength+1,1) bQfunction_Vector[1:qLength] = (dist_to_s_target:-1:dist_to_s_target-qLength+1)*0.1 @@ -135,10 +131,8 @@ function coeffConstraintCost(oldTraj::OldTrajectory, lapStatus::LapStatus, mpcCo end - mpcCoeff.coeffCost = coeffCost + mpcCoeff.coeffCost = coeffCost mpcCoeff.coeffConst = coeffConst - #mpcCoeff = MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) - #return mpcCoeff - #return MpcCoeff(coeffCost, coeffConst, mpcCoeff.order, mpcCoeff.pLength) + nothing end diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 995fc61c..7dad5786 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -6,6 +6,7 @@ function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{F zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line + # Save all data in oldTrajectory: if lapStatus.currentLap == 1 # if it's the first lap oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything @@ -23,25 +24,20 @@ function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{F oldTraj.oldInput[:,:,1] = uCurr_export oldTraj.oldCost[1] = costLap end - #println(size(save_oldTraj)) - #println(size(oldTraj.oldTraj)) - #save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] - - end function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) mpcParams.N = 10 mpcParams.nz = 4 - mpcParams.Q = [0.0,10.0,0.1,1.0] # put weights on ey, epsi and v - mpcParams.Q_term = [0.1,0.1,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) + mpcParams.Q = [0.0,10.0,0.0,0.1] # put weights on ey, epsi and v + mpcParams.Q_term = [0.1,0.01,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 0*[0,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1*[1,1] # cost matrix for derivative cost of inputs + mpcParams.QderivU = 1.0*[1,1] # cost matrix for derivative cost of inputs mpcParams.vPathFollowing = 1.0 # reference speed for first lap of path following - trackCoeff.nPolyCurvature = 5 # 4th order polynomial for curvature approximation + trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.width = 0.4 # width of the track (0.5m) @@ -59,7 +55,10 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP posInfo.s_target = 31.62#25.62#29.491949#13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) + oldTraj.oldTraj[:,1,1] = 1:buffersize + oldTraj.oldTraj[:,1,2] = 1:buffersize oldTraj.oldInput = zeros(buffersize,2,2) + oldTraj.oldCost = 100*ones(Int64,2) # dummies for initialization mpcCoeff.order = 5 mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) @@ -125,30 +124,34 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara end -function simModel(zNext::Array{Float64},z::Array{Float64},u::Array{Float64},modelParams::ModelParams,trackCoeff::TrackCoeff) - - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - dt = modelParams.dt - l_A = modelParams.l_A - l_B = modelParams.l_B - s = z[1] - c = 0 - for i=trackCoeff.nPolyCurvature:-1:0 - c += s^i * trackCoeff.coeffCurvature[trackCoeff.nPolyCurvature+1-i] - end - # println("Sim Model, c = $c") - #c = ([s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s 1] * coeff'')[1] - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - dsdt = z[4] *cos(z[3]+bta)/(1-z[2]*c) +function simModel(z::Array{Float64},u::Array{Float64},modelParams::ModelParams,trackCoeff::TrackCoeff) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + println("SIMULATING: u = $u, z0 = $z") - zNext[1] = z[1] + dt*dsdt # s - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_A*sin(bta)-dsdt*c) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + dt = modelParams.dt/10 + l_A = modelParams.l_A + l_B = modelParams.l_B + + zNext = copy(z) + for i=1:10 + s = zNext[1] + c = 0 + for j=trackCoeff.nPolyCurvature:-1:0 + c += s^j * trackCoeff.coeffCurvature[trackCoeff.nPolyCurvature+1-j] + end + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + dsdt = zNext[4] *cos(zNext[3]+bta)/(1-zNext[2]*c) + + zNext[1] = zNext[1] + dt*dsdt # s + zNext[2] = zNext[2] + dt*(zNext[4]*sin(zNext[3] + bta)) # y + zNext[3] = zNext[3] + dt*(zNext[4]/l_A*sin(bta)-dsdt*c) # psi + zNext[4] = zNext[4] + dt*(u[1] - 0.63 * zNext[4]^2 * sign(zNext[4])) # v + end - nothing + return zNext end function extendOldTraj(oldTraj::OldTrajectory,posInfo::PosInfo,zCurr::Array{Float64,2}) diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 3e113d38..9d077c2b 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -89,7 +89,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # State cost # --------------------------------- if lapStatus.currentLap <= 1 # if we're in the first lap, just do path following - @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[j,i]-z_Ref[j,i])^2,j=1:N+1},i=1:4}) # Follow trajectory + @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:4}) # Follow trajectory else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) #@NLexpression(mdl.mdl, costZ_h, 0) # zero state cost after crossing the finish line From 6305f58674f234480f15b40a2c0fba680f00c6c0 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 23 Sep 2016 17:27:22 -0700 Subject: [PATCH 032/183] Working version of LMPC. Works well with wide track (about 0.6m width). --- eval_sim/create_track.py | 38 ++++++------- eval_sim/create_track_bezier2.py | 36 ++++++++++++ eval_sim/eval_sim.jl | 55 +++++++++++-------- workspace/src/barc/src/LMPC_node.jl | 13 ++--- .../src/barc/src/Localization_helpers.py | 24 ++++---- workspace/src/barc/src/barc_simulator.jl | 2 +- workspace/src/barc/src/helper/functions.jl | 8 +-- 7 files changed, 110 insertions(+), 66 deletions(-) create mode 100644 eval_sim/create_track_bezier2.py diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index 3d3951e4..65671c41 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -23,26 +23,26 @@ def add_curve(theta, length, angle): theta = array([0]) -# theta = add_curve(theta,10,0) -# theta = add_curve(theta,50,-2*pi/3) -# #theta = add_curve(theta,30,0) -# theta = add_curve(theta,70,pi) -# theta = add_curve(theta,60,-5*pi/6) -# theta = add_curve(theta,10,0) -# theta = add_curve(theta,30,-pi/2) -# theta = add_curve(theta,40,0) -# theta = add_curve(theta,20,-pi/4) -# theta = add_curve(theta,20,pi/4) -# theta = add_curve(theta,50,-pi/2) -# theta = add_curve(theta,22,0) -# theta = add_curve(theta,30,-pi/2) -# theta = add_curve(theta,14,0) - +theta = add_curve(theta,30,0) +theta = add_curve(theta,60,-2*pi/3) +theta = add_curve(theta,90,pi) +theta = add_curve(theta,80,-5*pi/6) +theta = add_curve(theta,10,0) +theta = add_curve(theta,50,-pi/2) theta = add_curve(theta,50,0) -theta = add_curve(theta,100,-pi) -theta = add_curve(theta,100,0) -theta = add_curve(theta,100,-pi) -theta = add_curve(theta,49,0) +theta = add_curve(theta,40,-pi/4) +theta = add_curve(theta,30,pi/4) +theta = add_curve(theta,20,0) +theta = add_curve(theta,50,-pi/2) +theta = add_curve(theta,25,0) +theta = add_curve(theta,50,-pi/2) +theta = add_curve(theta,28,0) + +# theta = add_curve(theta,50,0) +# theta = add_curve(theta,100,-pi) +# theta = add_curve(theta,100,0) +# theta = add_curve(theta,100,-pi) +# theta = add_curve(theta,49,0) for i in range(0,size(theta)): x = hstack((x, x[-1] + cos(theta[i])*ds)) diff --git a/eval_sim/create_track_bezier2.py b/eval_sim/create_track_bezier2.py new file mode 100644 index 00000000..61dc43a9 --- /dev/null +++ b/eval_sim/create_track_bezier2.py @@ -0,0 +1,36 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.06 +theta = 0 +d_theta = 0 + +N = 40 + +def create_bezier(p0,p1,p2,p3,dt): + t = arange(0,1+dt,dt)[:,None] + p = (1-t)**3*p0 + 3*(1-t**2)*t*p1+3*(1-t)*t**2*p2+t**3*p3 + return p + +p0 = array([[0,0], + [0,-1], + [0,-2]]) +p1 = array([[1,0], + [-1,-1]]) + +p2 = 2*p0[1,:] - p1[1,:] +p3 = p0[1,:] +for i in range(1,size(p0,0)-2): + p3 = vstack((p3,p0[i,:])) + p2 = vstack((p2,2*p0[i+1]-p1[i+1])) + +p = p0[0,:] +for i in range(0,size(p0,0)-1): + p = vstack((p,create_bezier(p0[i,:],p1[i,:],p2[i,:],p3[i,:].0.01))) + +plt.plot(p[:,0],p[:,1],'-',p0[:,0],p0[:,1],'*') +plt.grid() +plt.axis('equal') +plt.show() diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 9f4ecac7..2fc7ed60 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -41,7 +41,7 @@ function eval_sim() cmd_log = d["cmd_log"] t0 = est.t[1] - track = create_track(0.2) + track = create_track(0.3) figure() plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") @@ -109,7 +109,7 @@ function eval_LMPC() legend(["psi_true","psi_est"]) grid() - track = create_track(0.2) + track = create_track(0.3) figure() hold(1) plot(x_est[:,1],x_est[:,2],"-o") @@ -122,16 +122,14 @@ function eval_LMPC() # #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] # #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") # end - # for i=1:size(x_est,1) - # if i%4==0 - # z_pred = zeros(11,4) - # z_pred[1,:] = x_est[i,:] - # for j=2:11 - # z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) - # end - # plot(z_pred[:,1],z_pred[:,2],"-*") - # end - # end + for i=1:4:size(x_est,1) + z_pred = zeros(11,4) + z_pred[1,:] = x_est[i,:] + for j=2:11 + z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-*") + end # for i=1:size(x_est,1) # s = 0.4:.1:2.5 @@ -142,10 +140,20 @@ function eval_LMPC() # end grid() + rg = 100:500 + figure() + plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") + for i=100:5:500 + plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") + end figure() + ax1=subplot(211) plot(t-t0,state,"-o",t-t0,f_z,"-*") legend(["s","ey","epsi","v","s_pred","ey_pred","epsi_pred","v_pred"]) grid() + subplot(212,sharex = ax1) + plot(t-t0,reshape(sol_u[1,:,:],2,size(sol_u,3))') + grid() figure() plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") @@ -313,19 +321,20 @@ function create_track(w) theta = [0.0] # SOPHISTICATED TRACK - add_curve(theta,60,0.0) - add_curve(theta,50,-2*pi/3) - add_curve(theta,70,pi) - add_curve(theta,60,-5*pi/6) + add_curve(theta,30,0.0) + add_curve(theta,60,-2*pi/3) + add_curve(theta,90,pi) + add_curve(theta,80,-5*pi/6) add_curve(theta,10,0.0) - add_curve(theta,30,-pi/2) - add_curve(theta,90,0.0) - add_curve(theta,20,-pi/4) - add_curve(theta,20,pi/4) add_curve(theta,50,-pi/2) - add_curve(theta,22,0.0) - add_curve(theta,30,-pi/2) - add_curve(theta,14,0.0) + add_curve(theta,50,0.0) + add_curve(theta,40,-pi/4) + add_curve(theta,30,pi/4) + add_curve(theta,20,0.0) + add_curve(theta,50,-pi/2) + add_curve(theta,25,0.0) + add_curve(theta,50,-pi/2) + add_curve(theta,28,0.0) # SIMPLE track # add_curve(theta,50,0) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index a76ae35e..3bd4f50d 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -32,7 +32,7 @@ end function main() println("now starting the node") - buffersize = 700 # size of oldTraj buffers + buffersize = 1000 # size of oldTraj buffers # Create data to be saved log_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps @@ -140,7 +140,7 @@ function main() trackCoeff.coeffCurvature = coeffCurvature_update # Simulate model for next input - #pred_z = simModel(z_est,uCurr[i,:][:],modelParams,trackCoeff) + pred_z = simModel(z_est,uCurr[i,:][:],modelParams,trackCoeff) # this simulation returns the predicted state at when the next command is going to be sent. This predicted state is used for # the MPC control input calculation. @@ -198,15 +198,13 @@ function main() # Solve the MPC problem tic() - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') + #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') tt = toq() # Write in current input information uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] - #println("trackCoeff = $trackCoeff") println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i+1,:]), t = $tt s") - # ... and publish data zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) println("\n") @@ -226,13 +224,12 @@ function main() log_state_x[k,:] = x_est log_coeffX[k,:] = coeffX log_coeffY[k,:] = coeffY - println("pred_z = $pred_z") # publish command from last calculation cmd.motor = mpcSol.a_x cmd.servo = mpcSol.d_f publish(pub, cmd) - + println("z_pred = $(pred_z')") else println("No estimation data received!") end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index ecf10c06..15024c9b 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -106,19 +106,21 @@ def create_track(self): theta = array([0]) # Sophisticated racetrack: length = 25.62m - theta = add_curve(theta,60,0) - theta = add_curve(theta,50,-2*pi/3) - theta = add_curve(theta,70,pi) - theta = add_curve(theta,60,-5*pi/6) + theta = add_curve(theta,30,0) + theta = add_curve(theta,60,-2*pi/3) + #theta = add_curve(theta,30,0) + theta = add_curve(theta,90,pi) + theta = add_curve(theta,80,-5*pi/6) theta = add_curve(theta,10,0) - theta = add_curve(theta,30,-pi/2) - theta = add_curve(theta,90,0) - theta = add_curve(theta,20,-pi/4) - theta = add_curve(theta,20,pi/4) theta = add_curve(theta,50,-pi/2) - theta = add_curve(theta,22,0) - theta = add_curve(theta,30,-pi/2) - theta = add_curve(theta,14,0) + theta = add_curve(theta,50,0) + theta = add_curve(theta,40,-pi/4) + theta = add_curve(theta,30,pi/4) + theta = add_curve(theta,20,0) + theta = add_curve(theta,50,-pi/2) + theta = add_curve(theta,25,0) + theta = add_curve(theta,50,-pi/2) + theta = add_curve(theta,28,0) # SIMPLE RACETRACK (smooth curves): length = 24.0m # theta = add_curve(theta,50,0) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index e7a095b5..bac6160c 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -106,7 +106,7 @@ function main() s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) z_current = zeros(60000,4) - z_current[1,:] = [0.05 0.0 0.0 0.0] + z_current[1,:] = [0.1 0.0 0.0 0.0] dt = 0.01 loop_rate = Rate(1/dt) diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 7dad5786..0b3bd5d0 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -31,15 +31,15 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.N = 10 mpcParams.nz = 4 mpcParams.Q = [0.0,10.0,0.0,0.1] # put weights on ey, epsi and v - mpcParams.Q_term = [0.1,0.01,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) + mpcParams.Q_term = 1*[0.1,0.01,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f - mpcParams.QderivZ = 0*[0,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivZ = 1.0*[0,0.0,0.1,0] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1,1] # cost matrix for derivative cost of inputs mpcParams.vPathFollowing = 1.0 # reference speed for first lap of path following trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.width = 0.4 # width of the track (0.5m) + trackCoeff.width = 0.6 # width of the track (0.5m) modelParams.u_lb = ones(mpcParams.N,1) * [-1.0 -pi/6] # lower bounds on steering modelParams.u_ub = ones(mpcParams.N,1) * [1.2 pi/6] # upper bounds @@ -52,7 +52,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP modelParams.dt = 0.1 posInfo.s_start = 0.0 - posInfo.s_target = 31.62#25.62#29.491949#13.20#10.281192 + posInfo.s_target = 36.84#31.62#25.62#29.491949#13.20#10.281192 oldTraj.oldTraj = zeros(buffersize,4,2) oldTraj.oldTraj[:,1,1] = 1:buffersize From ff13d4b8a9dec4bdd657b6ad516a7fb8b43a1f8f Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 23 Sep 2016 18:52:51 -0700 Subject: [PATCH 033/183] Modified state estimation: predicts state by 0.1s (linear prediction) --- eval_sim/eval_sim.jl | 4 +- workspace/src/barc/src/LMPC_node.jl | 35 +++++------ workspace/src/barc/src/barc_simulator.jl | 14 ++--- .../src/barc/src/state_estimation_KinBkMdl.py | 27 ++++++--- workspace/src/barc/src/system_models.py | 59 ++++++++++++++++++- 5 files changed, 99 insertions(+), 40 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 2fc7ed60..f572219c 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -4,7 +4,7 @@ using HDF5, JLD, ProfileView type Measurements{T} i::Int64 # measurement counter - t::Array{T} # time data + t::Array{Float64} # time data z::Array{T} # measurement values end @@ -48,7 +48,7 @@ function eval_sim() grid(1) title("x-y-view") axis("equal") - legend(["real state","GPS meas","estimate"]) + legend(["Real state","GPS meas","estimate"]) figure() plot(z.t-t0,z.z[:,3],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) grid(1) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 3bd4f50d..6b1921fd 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -126,26 +126,19 @@ function main() if z_est[1] > 0 # check if data has been received (s > 0) # publish command from last calculation - # cmd.motor = mpcSol.a_x - # cmd.servo = mpcSol.d_f - # publish(pub, cmd) + cmd.motor = mpcSol.a_x + cmd.servo = mpcSol.d_f + publish(pub, cmd) # ============================= Initialize iteration parameters ============================= lapStatus.currentIt += 1 # count iteration i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr[i,:] = z_est # update state information: s, e_y, e_psi, v + zCurr[i,:] = z_est # update state information: s, e_y, e_psi, v (actually predicted by Kalman filter!) posInfo.s = z_est[1] # update position info posInfo.s_start = s_start_update[1] trackCoeff.coeffCurvature = coeffCurvature_update - # Simulate model for next input - pred_z = simModel(z_est,uCurr[i,:][:],modelParams,trackCoeff) - - # this simulation returns the predicted state at when the next command is going to be sent. This predicted state is used for - # the MPC control input calculation. - - # ======================================= Lap trigger ======================================= # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... @@ -198,8 +191,8 @@ function main() # Solve the MPC problem tic() - #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') + #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') tt = toq() # Write in current input information @@ -208,7 +201,8 @@ function main() zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) println("\n") - + #println("Starting logging") + # Logging k = k + 1 # counter log_state[k,:] = z_est @@ -222,14 +216,15 @@ function main() log_s_start[k] = posInfo.s_start log_pred_z[k,:] = pred_z log_state_x[k,:] = x_est - log_coeffX[k,:] = coeffX - log_coeffY[k,:] = coeffY + #log_coeffX[k,:] = coeffX + #log_coeffY[k,:] = coeffY + #println("Finished logging") # publish command from last calculation - cmd.motor = mpcSol.a_x - cmd.servo = mpcSol.d_f - publish(pub, cmd) - println("z_pred = $(pred_z')") + #cmd.motor = mpcSol.a_x + #cmd.servo = mpcSol.d_f + #publish(pub, cmd) + #println("z_pred = $(pred_z')") else println("No estimation data received!") end diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index bac6160c..b9326e18 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -43,11 +43,11 @@ function clean_up(m::Measurements) end buffersize = 60000 -gps_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,1)) -est_meas = Measurements{Float32}(0,zeros(buffersize,1),zeros(Float32,buffersize,4)) -cmd_log = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,2)) -z_real = Measurements{Float64}(0,zeros(buffersize,1),zeros(buffersize,4)) +gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) +est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) +cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,4)) z_real.t[1] = time() imu_meas.t[1] = time() @@ -84,9 +84,9 @@ function ECU_callback(msg::ECU) end function est_callback(msg::Z_KinBkMdl) - t = time() + global est_meas est_meas.i += 1 - est_meas.t[est_meas.i] = t + est_meas.t[est_meas.i] = time() est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] end diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py index 70489b8d..d53ed0f0 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl.py @@ -21,7 +21,7 @@ from barc.msg import ECU, Encoder, Z_KinBkMdl from numpy import pi, cos, sin, eye, array, zeros, unwrap from observers import kinematicLuembergerObserver, ekf -from system_models import f_KinBkMdl, h_KinBkMdl +from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_predictive, h_KinBkMdl_predictive from tf import transformations from numpy import unwrap, diag from os.path import expanduser @@ -184,11 +184,13 @@ def state_estimation(): t0 = time.time() # estimation variables for Luemberger observer - z_EKF = zeros(4) + #z_EKF = zeros(4) + z_EKF = zeros(8) # estimation variables for EKF - P = eye(4) # initial dynamics coveriance matrix + P = eye(8) # initial dynamics coveriance matrix Q = (q_std**2)*eye(4) # process noise coveriance matrix + Q_predictive= (q_std**2)*eye(8) # R = (r_std**2)*eye(4) # measurement noise coveriance matrix if est_mode==1: R = diag([gps_std,gps_std,psi_std,v_std])**2 @@ -203,11 +205,21 @@ def state_estimation(): # start loop while not rospy.is_shutdown(): + # collect inputs + u = array([ d_f, acc ]) + args = (u,vhMdl,dt) + # publish state estimate - (x_e, y_e, psi_e, v_e) = z_EKF + #(x_e, y_e, psi_e, v_e) = z_EKF + (x_e, y_e, psi_e, v_e, x_e_pred, y_e_pred, psi_e_pred, v_e_pred) = z_EKF + + # predict state in 0.1 seconds + #z_pred = f_KinBkMdl(z_EKF,u,vhMdl,0.1) # publish information - state_pub.publish( Z_KinBkMdl(x_e, y_e, psi_e, v_e) ) + #state_pub.publish( Z_KinBkMdl(x_e, y_e, psi_e, v_e) ) + #state_pub.publish( Z_KinBkMdl(z_pred[0],z_pred[1],z_pred[2],z_pred[3])) + state_pub.publish( Z_KinBkMdl( x_e_pred, y_e_pred, psi_e_pred, v_e_pred )) # collect measurements, inputs, system properties @@ -218,11 +230,8 @@ def state_estimation(): elif est_mode==3: y = array([x_meas, y_meas]) - # collect inputs - u = array([ d_f, acc ]) - args = (u,vhMdl,dt) # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) + (z_EKF,P) = ekf(f_KinBkMdl_predictive, z_EKF, P, h_KinBkMdl_predictive, y, Q_predictive, R, args ) # wait diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 3c258677..98200234 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -221,7 +221,48 @@ def f_KinBkMdl(z,u,vhMdl, dt): v_next = v + dt*(a - 0.63*sign(v)*v**2) return array([x_next, y_next, psi_next, v_next]) - + +def f_KinBkMdl_predictive(z,u,vhMdl, dt): + """ + process model + input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] + output: state at next time step z[k+1] + """ + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) + + # get states / inputs + x = z[0] + y = z[1] + psi = z[2] + v = z[3] + + x_pred = z[4] + y_pred = z[5] + psi_pred= z[6] + v_pred = z[7] + + d_f = u[0] + a = u[1] + + # extract parameters + (L_a, L_b) = vhMdl + + # compute slip angle + bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + + # compute next state + x_next = x + dt*( v*cos(psi + bta) ) + y_next = y + dt*( v*sin(psi + bta) ) + psi_next = psi + dt*v/L_b*sin(bta) + v_next = v + dt*(a - 0.63*sign(v)*v**2) + + x_next_pred = x_next + 0.1*( v*cos(psi + bta) ) + y_next_pred = y_next + 0.1*( v*sin(psi + bta) ) + psi_next_pred = psi_next + 0.1*v/L_b*sin(bta) + v_next_pred = v_next + 0.1*(a - 0.63*sign(v)*v**2) + + return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) + def h_KinBkMdl(x): """ measurement model @@ -236,4 +277,18 @@ def h_KinBkMdl(x): C = array([[1, 0, 0, 0], [0, 1, 0, 0]]) return dot(C, x) - + +def h_KinBkMdl_predictive(x): + """ + measurement model + """ + # For GPS, IMU and encoders: + # C = array([[1, 0, 0, 0], + # [0, 1, 0, 0], + # [0, 0, 1, 0], + # [0, 0, 0, 1]]) + + # For GPS only: + C = array([[1, 0, 0, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0, 0, 0]]) + return dot(C, x) From 4303291f8ac81b2f1169a0da156f917a259de2b2 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 27 Sep 2016 18:18:38 -0700 Subject: [PATCH 034/183] Added dynamic model to state estimation. Added evaluation functions to eval_sim. --- eval_sim/eval_sim.jl | 15 ++ workspace/src/barc/CMakeLists.txt | 1 + workspace/src/barc/launch/barc_sim.launch | 29 +++ workspace/src/barc/msg/Z_DynBkMdl.msg | 6 + workspace/src/barc/src/barc_simulator.jl | 26 ++- .../src/barc/src/state_estimation_DynBkMdl.py | 167 ++++++++++-------- workspace/src/barc/src/system_models.py | 77 +++++++- 7 files changed, 237 insertions(+), 84 deletions(-) create mode 100644 workspace/src/barc/msg/Z_DynBkMdl.msg diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index f572219c..a4def5b6 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -35,6 +35,7 @@ function eval_sim() d = load(log_path) est = d["estimate"] + est_dyn = d["estimate_dyn"] imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] z = d["z"] @@ -49,6 +50,20 @@ function eval_sim() title("x-y-view") axis("equal") legend(["Real state","GPS meas","estimate"]) + + figure() + plot(est_dyn.t,est_dyn.z,"-*",est.t,est.z,"--",z.t,z.z,"-") + grid() + legend(["x","y","v_x","v_y","psi","psi_dot"]) + + figure() + plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,3],est_dyn.t,est_dyn.z[:,5:6],"-*") + legend(["psi_imu","psi_dot_imu","psi_true","psi_est","psi_dot_est"]) + grid() + + figure() + plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*",z.z[:,1],z.z[:,2],"-") + figure() plot(z.t-t0,z.z[:,3],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) grid(1) diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index 1dc489a5..ef279143 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -53,6 +53,7 @@ add_message_files( Encoder.msg ECU.msg Z_KinBkMdl.msg + Z_DynBkMdl.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 03ebb85e..71fa56dd 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -4,6 +4,20 @@ + + + + + + + + + + + + + + @@ -25,6 +39,21 @@ + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/msg/Z_DynBkMdl.msg b/workspace/src/barc/msg/Z_DynBkMdl.msg new file mode 100644 index 00000000..b3e2a3b4 --- /dev/null +++ b/workspace/src/barc/msg/Z_DynBkMdl.msg @@ -0,0 +1,6 @@ +float32 x +float32 y +float32 v_x +float32 v_y +float32 psi +float32 psi_dot diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index b9326e18..c2fd6342 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -14,7 +14,7 @@ =# using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @@ -44,14 +44,16 @@ end buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) +est_meas_dyn = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,6)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,4)) z_real.t[1] = time() imu_meas.t[1] = time() est_meas.t[1] = time() +est_meas_dyn.t[1] = time() cmd_log.t[1] = time() function simModel(z,u,dt,l_A,l_B) @@ -90,6 +92,13 @@ function est_callback(msg::Z_KinBkMdl) est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] end +function est_dyn_callback(msg::Z_DynBkMdl) + global est_meas_dyn + est_meas_dyn.i += 1 + est_meas_dyn.t[est_meas_dyn.i] = time() + est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x msg.y msg.v_x msg.v_y msg.psi msg.psi_dot] +end + function main() # initiate node, set up publisher / subscriber topics init_node("barc_sim") @@ -104,9 +113,10 @@ function main() s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) + s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=10) z_current = zeros(60000,4) - z_current[1,:] = [0.1 0.0 0.0 0.0] + z_current[1,:] = [0.2 0.0 0.0 0.0] dt = 0.01 loop_rate = Rate(1/dt) @@ -152,12 +162,14 @@ function main() # IMU measurements imu_data = Imu() imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds - yaw = z_current[i,3] + 0*(randn()*0.05 + imu_drift) + yaw = z_current[i,3] + 0*(randn()*0.05 + imu_drift) + yaw_dot = (z_current[i,3]-z_current[i-1,3])/dt imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) + imu_data.angular_velocity = Vector3(0,0,yaw_dot) if i%2 == 0 imu_meas.i += 1 imu_meas.t[imu_meas.i] = t - imu_meas.z[imu_meas.i] = yaw + imu_meas.z[imu_meas.i,:] = [yaw yaw_dot] publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end @@ -181,15 +193,15 @@ function main() clean_up(gps_meas) clean_up(est_meas) + clean_up(est_meas_dyn) clean_up(imu_meas) clean_up(cmd_log) z_real.z[1:i-1,:] = z_current[1:i-1,:] z_real.i = i clean_up(z_real) - # Save simulation data to file log_path = "$(homedir())/simulations/output.jld" - save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"imu_meas",imu_meas,"cmd_log",cmd_log) + save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") #writedlm(log_path,z_current[1:i-1,:]) end diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index c3e0b57b..a5a366d8 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -16,17 +16,17 @@ import rospy import time import os -from barc.msg import ECU, Encoder +from barc.msg import ECU, Encoder, Z_DynBkMdl from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 -from numpy import pi, cos, sin, eye, array, zeros +from numpy import pi, cos, sin, eye, array, zeros, diag from ekf import ekf -from system_models import f_3s, h_3s +from system_models import f_DynBkMdl, h_DynBkMdl from tf import transformations from numpy import unwrap # input variables -d_f = 0 +d_f = 0 FxR = 0 # raw measurement variables @@ -34,14 +34,17 @@ (roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) # from encoder -v_x_enc = 0 -t0 = time.time() -n_FL = 0 # counts in the front left tire -n_FR = 0 # counts in the front right tire -n_FL_prev = 0 -n_FR_prev = 0 -r_tire = 0.04 # radius from tire center to perimeter along magnets [m] -dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge +v_x_enc = 0 +t0 = time.time() +n_FL = 0 # counts in the front left tire +n_FR = 0 # counts in the front right tire +n_FL_prev = 0 +n_FR_prev = 0 +r_tire = 0.04 # radius from tire center to perimeter along magnets [m] +dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge + +x_meas = 0 +y_meas = 0 # ecu command update def ecu_callback(data): @@ -49,6 +52,13 @@ def ecu_callback(data): FxR = data.motor # input motor force [N] d_f = data.servo # input steering angle [rad] +# ultrasound gps data +def gps_callback(data): + # units: [rad] and [rad/s] + global x_meas, y_meas + x_meas = data.x/100 # data is given in cm + y_meas = data.y/100 + # imu measurement update def imu_callback(data): # units: [rad] and [rad/s] @@ -73,50 +83,51 @@ def imu_callback(data): # encoder measurement update def enc_callback(data): - global v_x_enc, d_f, t0 - global n_FL, n_FR, n_FL_prev, n_FR_prev + global v_x_enc, d_f, t0 + global n_FL, n_FR, n_FL_prev, n_FR_prev - n_FL = data.FL - n_FR = data.FR + n_FL = data.FL + n_FR = data.FR - # compute time elapsed - tf = time.time() - dt = tf - t0 - - # if enough time elapse has elapsed, estimate v_x - dt_min = 0.20 - if dt >= dt_min: - # compute speed : speed = distance / time - v_FL = float(n_FL- n_FL_prev)*dx_qrt/dt - v_FR = float(n_FR- n_FR_prev)*dx_qrt/dt + # compute time elapsed + tf = time.time() + dt = tf - t0 + + # if enough time elapse has elapsed, estimate v_x + dt_min = 0.20 + if dt >= dt_min: + # compute speed : speed = distance / time + v_FL = float(n_FL- n_FL_prev)*dx_qrt/dt + v_FR = float(n_FR- n_FR_prev)*dx_qrt/dt - # update encoder v_x, v_y measurements - # only valid for small slip angles, still valid for drift? - v_x_enc = (v_FL + v_FR)/2.0*cos(d_f) + # update encoder v_x, v_y measurements + # only valid for small slip angles, still valid for drift? + v_x_enc = (v_FL + v_FR)/2.0*cos(d_f) - # update old data - n_FL_prev = n_FL - n_FR_prev = n_FR - t0 = time.time() + # update old data + n_FL_prev = n_FL + n_FR_prev = n_FR + t0 = time.time() # state estimation node def state_estimation(): - # initialize node + # initialize node rospy.init_node('state_estimation', anonymous=True) # topic subscriptions / publications rospy.Subscriber('imu/data', Imu, imu_callback) rospy.Subscriber('encoder', Encoder, enc_callback) rospy.Subscriber('ecu', ECU, ecu_callback) - state_pub = rospy.Publisher('state_estimate', Vector3, queue_size = 10) + rospy.Subscriber('indoor_gps', Vector3, gps_callback) + state_pub = rospy.Publisher('state_estimate_dynamic', Z_DynBkMdl, queue_size = 10) - # get vehicle dimension parameters - L_a = rospy.get_param("L_a") # distance from CoG to front axel - L_b = rospy.get_param("L_b") # distance from CoG to rear axel + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel m = rospy.get_param("m") # mass of vehicle I_z = rospy.get_param("I_z") # moment of inertia about z-axis - vhMdl = (L_a, L_b, m, I_z) + vhMdl = (L_f, L_r, m, I_z) # get encoder parameters dt_vx = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x @@ -132,56 +143,74 @@ def state_estimation(): Ff = rospy.get_param("friction") # get EKF observer properties - q_std = rospy.get_param("state_estimation/q_std") # std of process noise - r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise - v_x_min = rospy.get_param("state_estimation/v_x_min") # minimum velociy before using EKF - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - t0 = time.time() + q_std = rospy.get_param("state_estimation_dynamic/q_std") # std of process noise + psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise + v_std = rospy.get_param("state_estimation_dynamic/v_std") + gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements + ang_v_std = rospy.get_param("state_estimation_dynamic/ang_v_std") # std of gps measurements + v_x_min = rospy.get_param("state_estimation_dynamic/v_x_min") # minimum velociy before using EKF + est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + t0 = time.time() # estimation variables for Luemberger observer - z_EKF = array([1.0, 0.0, 0.0]) + z_EKF = zeros(6) # estimation variables for EKF - P = eye(3) # initial dynamics coveriance matrix - Q = (q_std**2)*eye(3) # process noise coveriance matrix - R = (r_std**2)*eye(2) # measurement noise coveriance matrix - + P = eye(6) # initial dynamics coveriance matrix + #Q = (q_std**2)*eye(6) # process noise coveriance matrixif est_mode==1: + Q = diag([0.01,0.01,0.1,0.1,1.0,0.01]) # values derived from inspecting P matrix during Kalman filter running + + if est_mode==1: # use gps, IMU, and encoder + R = diag([gps_std,gps_std,psi_std,v_std])**2 + elif est_mode==2: # use IMU and encoder only + R = diag([psi_std,v_std])**2 + elif est_mode==3: # use gps only + R = (gps_std**2)*eye(2) + elif est_mode==4: # use gps and angular velocity + R = diag([gps_std,gps_std,ang_v_std])**2 + else: + rospy.logerr("No estimation mode selected.") + + running = False while not rospy.is_shutdown(): - # publish state estimate - (v_x, v_y, r) = z_EKF # note, r = EKF estimate yaw rate + # publish state estimate + (x,y,v_x,v_y,psi,psi_dot) = z_EKF # note, r = EKF estimate yaw rate # publish information - state_pub.publish( Vector3(v_x, v_y, r) ) + state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) # apply EKF - if v_x_enc > v_x_min: + #if v_x_enc > v_x_min: + if FxR > 0 or running: + running = True # get measurement - y = array([v_x_enc, w_z]) + y = array([x_meas,y_meas,w_z]) # define input u = array([ d_f, FxR ]) # build extra arguments for non-linear function - F_ext = array([ a0, Ff ]) - args = (u, vhMdl, TrMdl, F_ext, dt) + #F_ext = array([ a0, Ff ]) + args = (u, vhMdl, TrMdl, dt) # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_3s, z_EKF, P, h_3s, y, Q, R, args ) - + (z_EKF,P) = ekf(f_DynBkMdl, z_EKF, P, h_DynBkMdl, y, Q, R, args ) + else: - z_EKF[0] = float(v_x_enc) - z_EKF[2] = float(w_z) + z_EKF[0] = float(x_meas) + z_EKF[1] = float(y_meas) - # wait + # wait rate.sleep() if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 98200234..64e5bbbb 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -40,8 +40,8 @@ def f_2s(z, u, vhMdl, trMdl, dt, v_x): a_R = arctan(beta - b*r/v_x) # compute tire force - FyF = f_pajecka(trMdlFront, a_F) - FyR = f_pajecka(trMdlRear, a_R) + FyF = f_pacejka(trMdlFront, a_F) + FyR = f_pacejka(trMdlRear, a_R) # compute next state beta_next = beta + dt*(-r + (1/(m*v_x))*(FyF*cos(d_f)+FyR)) @@ -82,11 +82,11 @@ def f_3s(z, u, vhMdl, trMdl, F_ext, dt): # compute lateral tire force at the front TM_param = [B, C, mu*Fn] - FyF = -f_pajecka(TM_param, a_F) + FyF = -f_pacejka(TM_param, a_F) # compute lateral tire force at the rear # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pajecka(TM_param, a_R) + FyR_paj = -f_pacejka(TM_param, a_R) FyR_max = sqrt((mu*Fn)**2 - FxR**2) FyR = min(FyR_max, max(-FyR_max, FyR_paj)) @@ -135,11 +135,11 @@ def f_6s(z, u, vhMdl, trMdl, F_ext, dt): # compute lateral tire force at the front TM_param = [B, C, mu*Fn] - FyF = -f_pajecka(TM_param, a_F) + FyF = -f_pacejka(TM_param, a_F) # compute lateral tire force at the rear # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pajecka(TM_param, a_R) + FyR_paj = -f_pacejka(TM_param, a_R) FyR_max = sqrt((mu*Fn)**2 - FxR**2) Fy = array([FyR_max, FyR_paj]) idx = argmin(abs(Fy)) @@ -177,9 +177,9 @@ def h_3s(x): return dot(C, x) -def f_pajecka(trMdl, alpha): +def f_pacejka(trMdl, alpha): """ - f_pajecka = d*sin(c*atan(b*alpha)) + f_pacejka = d*sin(c*atan(b*alpha)) inputs : * trMdl := tire model, a list or tuple of parameters (b,c,d) @@ -222,6 +222,60 @@ def f_KinBkMdl(z,u,vhMdl, dt): return array([x_next, y_next, psi_next, v_next]) +def f_DynBkMdl(z,u,vhMdl,trMdl,dt): + x_I = z[0] + y_I = z[1] + v_x = z[2] + v_y = z[3] + psi = z[4] + psi_dot = z[5] + + d_f = u[0] + a = u[1] + + # extract parameters + (L_f,L_r,m,I_z) = vhMdl + (trMdlFront, trMdlRear) = trMdl + (B,C,mu) = trMdlFront + g = 9.81 + Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) + + # comptue the front/rear slip [rad/s] + # ref: Hindiyeh Thesis, p58 + a_F = 0 + a_R = 0 + if v_x != 0: + a_F = arctan((v_y + L_f*psi_dot)/v_x) - d_f + a_R = arctan((v_y - L_r*psi_dot)/v_x) + + #print "v_x = %f, v_y = %f, psi_dot = %f"%(v_x,v_y,psi_dot) + #print "a_F = %f, a_R = %f"%(a_F, a_R) + # compute lateral tire force at the front + TM_param = [B, C, mu*Fn] + FyF = -f_pacejka(TM_param, a_F) + + # compute lateral tire force at the rear + # ensure that magnitude of longitudinal/lateral force lie within friction circle + FyR_paj = -f_pacejka(TM_param, a_R) + FyR_max = sqrt((mu*Fn)**2 - a**2) # maximum tire force (resulting from friction and motor acceleration) + FyR = min(FyR_max, max(-FyR_max, FyR_paj)) + + C_alpha_f = 10 + C_alpha_r = 10 + + FyF = -C_alpha_f * a_F + FyR = -C_alpha_r * a_R + + # compute next state + x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) + y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) + v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) + v_y_next = v_y + dt * (1/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) + psi_next = psi + dt * (psi_dot) + psi_dot_next = psi_dot + dt * (1/I_z*(L_f*FyF*cos(d_f) - L_r*FyR)) + + return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next]) + def f_KinBkMdl_predictive(z,u,vhMdl, dt): """ process model @@ -263,6 +317,13 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt): return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) +def h_DynBkMdl(x): + # For GPS only: + C = array([[1, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1]]) + return dot(C, x) + def h_KinBkMdl(x): """ measurement model From af828ea35d64999c4f1494334405e8d14d54cc44 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 27 Sep 2016 19:54:19 -0700 Subject: [PATCH 035/183] added dynamic system to simulator. Not yet working properly. --- eval_sim/eval_sim.jl | 11 + workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/src/barc_simulator.jl | 3 - workspace/src/barc/src/barc_simulator_dyn.jl | 262 +++++++++++++++++++ workspace/src/barc/src/helper/functions.jl | 2 +- 5 files changed, 275 insertions(+), 5 deletions(-) create mode 100755 workspace/src/barc/src/barc_simulator_dyn.jl diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index f572219c..4c7b3d5d 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -39,9 +39,20 @@ function eval_sim() gps_meas = d["gps_meas"] z = d["z"] cmd_log = d["cmd_log"] + slip_a = d["slip_a"] t0 = est.t[1] track = create_track(0.3) + + figure() + plot(z.t-t0,z.z) + grid() + legend(["x","y","v_x","v_y","psi","psi_dot"]) + + figure() + plot(slip_a.t-t0,slip_a.z) + grid() + legend(["a_f","a_r"]) figure() plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 03ebb85e..fa8bba15 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -7,7 +7,7 @@ - + diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index b9326e18..b01e5a7e 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -133,9 +133,6 @@ function main() # update current state with a new row vector z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' z_real.t[i] = t - dist_traveled += z_current[i,4]*dt - - # Encoder measurements calculation dist_traveled += z_current[i,4]*dt #count the total traveled distance since the beginning of the simulation diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl new file mode 100755 index 00000000..d2ed09ef --- /dev/null +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -0,0 +1,262 @@ +#!/usr/bin/env julia + +#= + Licensing Information: You are free to use or extend these projects for + education or reserach purposes provided that (1) you retain this notice + and (2) you provide clear attribution to UC Berkeley, including a link + to http://barc-project.com + + Attibution Information: The barc project ROS code-base was developed + at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales + (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed + by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was + based on an open source project by Bruce Wootton +=# + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using sensor_msgs.msg +using JLD + +u_current = zeros(2,1) + +t = 0 + + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data + z::Array{T} # measurement values +end + +type ModelParams + l_A::Float64 + l_B::Float64 + m::Float64 + I_z::Float64 +end + +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + +buffersize = 60000 +gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) +est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) +cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) +slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + +z_real.t[1] = time() +slip_a.t[1] = time() +imu_meas.t[1] = time() +est_meas.t[1] = time() +cmd_log.t[1] = time() + + +function pacejka(a) + B = 10#0.3 + C = 1.25 + mu = 0.234 + m = 1.98 + g = 9.81 + D = mu * m * g/2 + + C_alpha_f = D*sin(C*atan(B*a)) + return C_alpha_f +end + +function simDynModel_exact(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + dtn = dt/10 + t = 0:dtn:dt + z_final = z + ang = zeros(2) + for i=1:length(t) + z_final, ang = simDynModel(z_final,u,dtn,modelParams) + end + return z_final, ang +end + +function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + #m = 1 + #I_z = 1 + + a_F = 0 + a_R = 0 + if abs(z[3]) >= 0.1 + a_F = atan((z[4] + L_f*z[6])/z[3]) - u[2] + a_R = atan((z[4] - L_r*z[6])/z[3]) + end + + C_alpha_f = pacejka(a_F)*1 + C_alpha_r = pacejka(a_R)*1 + + FyF = -C_alpha_f# * a_F + FyR_paj = -C_alpha_r# * a_R + + mu = 0.234 + Fn = m*9.81 + FxR = u[1] + FyR_max = sqrt((mu*Fn)^2 - FxR^2) + FyR = min(FyR_max, max(-FyR_max, FyR_paj)) + + + zNext = z + # compute next state + zNext[1] = zNext[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) + zNext[2] = zNext[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) + zNext[3] = zNext[3] + dt * (u[1] + z[4]*z[6]) + zNext[4] = zNext[4] + dt * (2/m*(FyF*cos(u[2]) + FyR) - z[6]*z[3]) + zNext[5] = zNext[5] + dt * (z[6]) + zNext[6] = zNext[6] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) + + #zNext = zNext + 0*randn(1,4)*0.001 + return zNext, [a_F a_R] +end + +function ECU_callback(msg::ECU) + global u_current + u_current = [msg.motor, msg.servo] + cmd_log.i += 1 + cmd_log.t[cmd_log.i] = time() + cmd_log.z[cmd_log.i,:] = u_current' +end + +function est_callback(msg::Z_KinBkMdl) + global est_meas + est_meas.i += 1 + est_meas.t[est_meas.i] = time() + est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] +end + +function main() + # initiate node, set up publisher / subscriber topics + init_node("barc_sim") + pub_enc = Publisher("encoder", Encoder, queue_size=10) + pub_gps = Publisher("indoor_gps", Vector3, queue_size=10) + pub_imu = Publisher("imu/data", Imu, queue_size=10) + + + # read the axle distances from the launch file + l_A = get_param("L_a") # distance from CoG to front axel + l_B = get_param("L_b") # distance from CoG to rear axel + + s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) + s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) + + z_current = zeros(60000,6) + z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0] + slip_ang = zeros(60000,2) + + dt = 0.01 + loop_rate = Rate(1/dt) + + i = 2 + + dist_traveled = 0 + last_updated = 0 + + r_tire = 0.036 # radius from tire center to perimeter along magnets [m] + quarterCirc = 0.5 * pi * r_tire # length of a quarter of a tire, distance from one to the next encoder + + FL = 0 #front left wheel encoder counter + FR = 0 #front right wheel encoder counter + BL = 0 #back left wheel encoder counter + BR = 0 #back right wheel encoder counter + + imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) + + modelParams = ModelParams(0.125,0.125,1.98,0.24) # L_f, L_r, m, I_z + + println("Publishing sensor information. Simulator running.") + while ! is_shutdown() + + t = time() + # update current state with a new row vector + z_current[i,:],slip_ang[i,:] = simDynModel_exact(z_current[i-1,:],u_current', dt, modelParams) + println("z_current:") + println(z_current[i,:]) + println(slip_ang[i,:]) + + z_real.t[i] = t + slip_a.t[i] = t + + # Encoder measurements calculation + dist_traveled += z_current[i,3]*dt #count the total traveled distance since the beginning of the simulation + if dist_traveled - last_updated >= quarterCirc + last_updated = dist_traveled + FL += 1 + FR += 1 + BL += 1 + BR += 0 #no encoder on back right wheel + enc_data = Encoder(FL, FR, BL, BR) + publish(pub_enc, enc_data) #publish a message everytime the encoder counts up + end + + # IMU measurements + imu_data = Imu() + imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds + yaw = z_current[i,5] + 0*(randn()*0.05 + imu_drift) + imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) + if i%2 == 0 + imu_meas.i += 1 + imu_meas.t[imu_meas.i] = t + imu_meas.z[imu_meas.i] = yaw + publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" + # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) + end + + # GPS measurements + x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 1*randn()*2) + if i % 7 == 0 + gps_meas.i += 1 + gps_meas.t[gps_meas.i] = t + gps_meas.z[gps_meas.i,:] = [x y] + gps_data = Vector3(x,y,0) + publish(pub_gps, gps_data) + end + + i += 1 + rossleep(loop_rate) + end + + # Clean up buffers + + clean_up(gps_meas) + clean_up(est_meas) + clean_up(imu_meas) + clean_up(cmd_log) + z_real.z[1:i-1,:] = z_current[1:i-1,:] + slip_a.z[1:i-1,:] = slip_ang[1:i-1,:] + z_real.i = i + slip_a.i = i + clean_up(z_real) + clean_up(slip_a) + + # Save simulation data to file + log_path = "$(homedir())/simulations/output.jld" + save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) + println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") + #writedlm(log_path,z_current[1:i-1,:]) +end + +if ! isinteractive() + main() +end diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index 0b3bd5d0..dc835627 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -35,7 +35,7 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 1.0*[0,0.0,0.1,0] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1,1] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 1.0 # reference speed for first lap of path following + mpcParams.vPathFollowing = 0.2 # reference speed for first lap of path following trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) From 02a25a0e9bcb9192588c3d5ccc5d526b2ffa2a59 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 28 Sep 2016 12:16:14 -0700 Subject: [PATCH 036/183] Changed simulation model to dynamic model (including dynamic steering angle) and adapted evaluation functions. Dynamic model is working but not stable for high speeds (also because estimation does not work properly for high speeds) --- eval_sim/eval_sim.jl | 41 +++++++++++------ workspace/src/barc/src/barc_simulator_dyn.jl | 46 +++++++++----------- workspace/src/barc/src/helper/functions.jl | 20 ++++----- workspace/src/barc/src/system_models.py | 8 ++-- 4 files changed, 61 insertions(+), 54 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 4c7b3d5d..b4046742 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -45,14 +45,19 @@ function eval_sim() track = create_track(0.3) figure() - plot(z.t-t0,z.z) + ax1=subplot(311) + plot(z.t-t0,z.z,"-o") grid() - legend(["x","y","v_x","v_y","psi","psi_dot"]) - - figure() - plot(slip_a.t-t0,slip_a.z) + legend(["x","y","v_x","v_y","psi","psi_dot","d_f"]) + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-o") + grid() + legend(["u","d_f"]) + subplot(313,sharex=ax1) + plot(slip_a.t-t0,slip_a.z,"-o") grid() legend(["a_f","a_r"]) + figure() plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") @@ -61,13 +66,10 @@ function eval_sim() axis("equal") legend(["Real state","GPS meas","estimate"]) figure() - plot(z.t-t0,z.z[:,3],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) + plot(z.t-t0,z.z[:,5],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) grid(1) legend(["Real psi","psi meas","estimate"]) - figure() - plot(z.t-t0,z.z[:,4]) - grid() - legend(["Velocity"]) + figure() plot(cmd_log.t-t0,cmd_log.z) legend(["a","d_f"]) @@ -98,6 +100,7 @@ function eval_LMPC() t0 = t[1] + figure() c = zeros(size(curv,1),1) for i=1:size(curv,1) s = state[i,1] @@ -116,7 +119,13 @@ function eval_LMPC() grid() figure() - plot(z.t-t0,z.z[:,3],t-t0,x_est[:,3]) + plot(z.t-t0,z.z[:,[1,2,5]],"-x",est.t-t0,est.z[:,1:3],"-*") + grid(1) + legend(["x","y","psi","x_est","y_est","psi_est"]) + + figure() + plot(z.t-t0,z.z[:,5],t-t0,x_est[:,3]) + title("Comparison heading angle") legend(["psi_true","psi_est"]) grid() @@ -124,8 +133,10 @@ function eval_LMPC() figure() hold(1) plot(x_est[:,1],x_est[:,2],"-o") + legend(["Estimated position"]) plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") axis("equal") + grid(1) # for i=1:size(x_est,1) # #dir = [cos(x_est[i,3]) sin(x_est[i,3])] # #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] @@ -149,11 +160,13 @@ function eval_LMPC() # y = ss*coeffY[i,:]' # plot(x,y) # end - grid() rg = 100:500 figure() plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") + title("Comparison states and prediction") + legend(["ey","epsi","v"]) + grid(1) for i=100:5:500 plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") end @@ -161,10 +174,10 @@ function eval_LMPC() ax1=subplot(211) plot(t-t0,state,"-o",t-t0,f_z,"-*") legend(["s","ey","epsi","v","s_pred","ey_pred","epsi_pred","v_pred"]) - grid() + grid(1) subplot(212,sharex = ax1) plot(t-t0,reshape(sol_u[1,:,:],2,size(sol_u,3))') - grid() + grid(1) figure() plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index d2ed09ef..8dcbb063 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -42,6 +42,7 @@ type ModelParams l_B::Float64 m::Float64 I_z::Float64 + v_steer::Float64 end # This function cleans the zeros from the type above once the simulation is finished @@ -55,7 +56,7 @@ gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) +z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,7)) slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) z_real.t[1] = time() @@ -64,14 +65,14 @@ imu_meas.t[1] = time() est_meas.t[1] = time() cmd_log.t[1] = time() - function pacejka(a) - B = 10#0.3 + B = 0.3#20 C = 1.25 mu = 0.234 m = 1.98 g = 9.81 D = mu * m * g/2 + D = D*100 C_alpha_f = D*sin(C*atan(B*a)) return C_alpha_f @@ -82,51 +83,44 @@ function simDynModel_exact(z::Array{Float64},u::Array{Float64},dt::Float64,model t = 0:dtn:dt z_final = z ang = zeros(2) - for i=1:length(t) + for i=1:length(t)-1 z_final, ang = simDynModel(z_final,u,dtn,modelParams) end return z_final, ang end function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + zNext::Array{Float64} L_f = modelParams.l_A L_r = modelParams.l_B m = modelParams.m I_z = modelParams.I_z - #m = 1 - #I_z = 1 + v_steer = modelParams.v_steer # 0.5 rad / 0.2 seconds a_F = 0 a_R = 0 - if abs(z[3]) >= 0.1 - a_F = atan((z[4] + L_f*z[6])/z[3]) - u[2] + if abs(z[3]) > 0.1 + a_F = atan((z[4] + L_f*z[6])/z[3]) - z[7] a_R = atan((z[4] - L_r*z[6])/z[3]) end - C_alpha_f = pacejka(a_F)*1 - C_alpha_r = pacejka(a_R)*1 + C_alpha_f = pacejka(a_F) + C_alpha_r = pacejka(a_R) FyF = -C_alpha_f# * a_F - FyR_paj = -C_alpha_r# * a_R - - mu = 0.234 - Fn = m*9.81 - FxR = u[1] - FyR_max = sqrt((mu*Fn)^2 - FxR^2) - FyR = min(FyR_max, max(-FyR_max, FyR_paj)) - + FyR = -C_alpha_r# * a_R zNext = z # compute next state zNext[1] = zNext[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) zNext[2] = zNext[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) - zNext[3] = zNext[3] + dt * (u[1] + z[4]*z[6]) - zNext[4] = zNext[4] + dt * (2/m*(FyF*cos(u[2]) + FyR) - z[6]*z[3]) + zNext[3] = zNext[3] + dt * (u[1] + z[4]*z[6] - 0.63*z[3]^2*sign(z[3])) + zNext[4] = zNext[4] + dt * (2/m*(FyF*cos(z[7]) + FyR) - z[6]*z[3]) zNext[5] = zNext[5] + dt * (z[6]) zNext[6] = zNext[6] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) + zNext[7] = zNext[7] + dt * v_steer * sign(u[2]-z[7]) - #zNext = zNext + 0*randn(1,4)*0.001 return zNext, [a_F a_R] end @@ -160,8 +154,8 @@ function main() s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) - z_current = zeros(60000,6) - z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0] + z_current = zeros(60000,7) + z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0] slip_ang = zeros(60000,2) dt = 0.01 @@ -182,7 +176,7 @@ function main() imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) - modelParams = ModelParams(0.125,0.125,1.98,0.24) # L_f, L_r, m, I_z + modelParams = ModelParams(0.125,0.125,1.98,0.24,0.5/0.2) # L_f, L_r, m, I_z, v_steer println("Publishing sensor information. Simulator running.") while ! is_shutdown() @@ -223,8 +217,8 @@ function main() end # GPS measurements - x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 1*randn()*2) + x = round(z_current[i,1]*100 + 0*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 0*randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index dc835627..c66e2e90 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -35,16 +35,16 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams.QderivZ = 1.0*[0,0.0,0.1,0] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1,1] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 0.2 # reference speed for first lap of path following + mpcParams.vPathFollowing = 0.6 # reference speed for first lap of path following trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) trackCoeff.width = 0.6 # width of the track (0.5m) - modelParams.u_lb = ones(mpcParams.N,1) * [-1.0 -pi/6] # lower bounds on steering + modelParams.u_lb = ones(mpcParams.N,1) * [0.0 -pi/6] # lower bounds on steering modelParams.u_ub = ones(mpcParams.N,1) * [1.2 pi/6] # upper bounds - modelParams.z_lb = ones(mpcParams.N+1,1)*[-Inf -trackCoeff.width/2 -Inf -Inf] # lower bounds on states - modelParams.z_ub = ones(mpcParams.N+1,1)*[Inf trackCoeff.width/2 Inf Inf] # upper bounds + modelParams.z_lb = ones(mpcParams.N+1,1)*[-Inf -Inf -Inf -0.1] # lower bounds on states + modelParams.z_ub = ones(mpcParams.N+1,1)*[ Inf Inf Inf 2.0] # upper bounds #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) modelParams.c0 = [1, 0.63, 1, 0] # BARC-specific parameters (measured) modelParams.l_A = 0.125 @@ -96,12 +96,12 @@ function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelPara setupperbound(m.u_Ol[j,i], modelParams.u_ub[j,i]) end end - # for i=1:4 - # for j=1:N+1 - # setlowerbound(m.z_Ol[j,i], modelParams.z_lb[j,i]) - # setupperbound(m.z_Ol[j,i], modelParams.z_ub[j,i]) - # end - # end + for i=1:4 + for j=1:N+1 + setlowerbound(m.z_Ol[j,i], modelParams.z_lb[j,i]) + setupperbound(m.z_Ol[j,i], modelParams.z_ub[j,i]) + end + end #@variable( m.mdl, 1 >= m.ParInt >= 0 ) @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 98200234..5a31a41a 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -256,10 +256,10 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt): psi_next = psi + dt*v/L_b*sin(bta) v_next = v + dt*(a - 0.63*sign(v)*v**2) - x_next_pred = x_next + 0.1*( v*cos(psi + bta) ) - y_next_pred = y_next + 0.1*( v*sin(psi + bta) ) - psi_next_pred = psi_next + 0.1*v/L_b*sin(bta) - v_next_pred = v_next + 0.1*(a - 0.63*sign(v)*v**2) + x_next_pred = x_next + 0.2*( v*cos(psi + bta) ) + y_next_pred = y_next + 0.2*( v*sin(psi + bta) ) + psi_next_pred = psi_next + 0.2*v/L_b*sin(bta) + v_next_pred = v_next + 0.2*(a - 0.63*sign(v)*v**2) return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) From 58bb34a628c8d08c588cecd5bcf29e3ea7aee94c Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 28 Sep 2016 16:40:41 -0700 Subject: [PATCH 037/183] Added dynamic estimator to dynamic simulation. Might need some tuning but is working. --- eval_sim/eval_sim.jl | 3 +- workspace/src/barc/launch/barc_sim.launch | 8 +- workspace/src/barc/msg/Z_DynBkMdl.msg | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 29 ++++--- workspace/src/barc/src/ekf.py | 79 ------------------- .../src/barc/src/state_estimation_DynBkMdl.py | 9 ++- workspace/src/barc/src/system_models.py | 47 ++++++----- 7 files changed, 59 insertions(+), 118 deletions(-) delete mode 100644 workspace/src/barc/src/ekf.py diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index a7b84ffb..e628ae72 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -69,8 +69,9 @@ function eval_sim() figure() plot(est_dyn.t,est_dyn.z,"-*",est.t,est.z,"--",z.t,z.z,"-") + title("Dyn. est. -*, est. --, real -") grid() - legend(["x","y","v_x","v_y","psi","psi_dot"]) + legend(["x","y","v_x","v_y","psi","psi_dot","x","y","psi","v","x","y","v_x","v_y","psi","psi_dot","d_f"]) figure() plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,3],est_dyn.t,est_dyn.z[:,5:6],"-*") diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index fcc53a5c..7cfe7580 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -27,7 +27,7 @@ - + @@ -39,7 +39,7 @@ - + @@ -48,9 +48,9 @@ - + - + diff --git a/workspace/src/barc/msg/Z_DynBkMdl.msg b/workspace/src/barc/msg/Z_DynBkMdl.msg index b3e2a3b4..4dc86872 100644 --- a/workspace/src/barc/msg/Z_DynBkMdl.msg +++ b/workspace/src/barc/msg/Z_DynBkMdl.msg @@ -3,4 +3,4 @@ float32 y float32 v_x float32 v_y float32 psi -float32 psi_dot +float32 psi_dot \ No newline at end of file diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 8dcbb063..c0d7a060 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -14,7 +14,7 @@ =# using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @@ -55,6 +55,7 @@ buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) +est_meas_dyn = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,6)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,7)) slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) @@ -63,6 +64,7 @@ z_real.t[1] = time() slip_a.t[1] = time() imu_meas.t[1] = time() est_meas.t[1] = time() +est_meas_dyn.t[1] = time() cmd_log.t[1] = time() function pacejka(a) @@ -105,11 +107,8 @@ function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams a_R = atan((z[4] - L_r*z[6])/z[3]) end - C_alpha_f = pacejka(a_F) - C_alpha_r = pacejka(a_R) - - FyF = -C_alpha_f# * a_F - FyR = -C_alpha_r# * a_R + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) zNext = z # compute next state @@ -132,6 +131,13 @@ function ECU_callback(msg::ECU) cmd_log.z[cmd_log.i,:] = u_current' end +function est_dyn_callback(msg::Z_DynBkMdl) + global est_meas_dyn + est_meas_dyn.i += 1 + est_meas_dyn.t[est_meas_dyn.i] = time() + est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x msg.y msg.v_x msg.v_y msg.psi msg.psi_dot] +end + function est_callback(msg::Z_KinBkMdl) global est_meas est_meas.i += 1 @@ -153,6 +159,7 @@ function main() s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) + s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=10) z_current = zeros(60000,7) z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0] @@ -184,9 +191,9 @@ function main() t = time() # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact(z_current[i-1,:],u_current', dt, modelParams) - println("z_current:") - println(z_current[i,:]) - println(slip_ang[i,:]) + p#rintln("z_current:") + #println(z_current[i,:]) + #println(slip_ang[i,:]) z_real.t[i] = t slip_a.t[i] = t @@ -208,6 +215,7 @@ function main() imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds yaw = z_current[i,5] + 0*(randn()*0.05 + imu_drift) imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) + imu_data.angular_velocity = Vector3(0,0,z_current[i,6]) if i%2 == 0 imu_meas.i += 1 imu_meas.t[imu_meas.i] = t @@ -235,6 +243,7 @@ function main() clean_up(gps_meas) clean_up(est_meas) + clean_up(est_meas_dyn) clean_up(imu_meas) clean_up(cmd_log) z_real.z[1:i-1,:] = z_current[1:i-1,:] @@ -246,7 +255,7 @@ function main() # Save simulation data to file log_path = "$(homedir())/simulations/output.jld" - save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) + save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") #writedlm(log_path,z_current[1:i-1,:]) end diff --git a/workspace/src/barc/src/ekf.py b/workspace/src/barc/src/ekf.py deleted file mode 100644 index 0fa7d1e7..00000000 --- a/workspace/src/barc/src/ekf.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from numpy import array, dot, eye, copy -from numpy import dot, zeros -from scipy.linalg import inv -import rospy - -def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): - """ - EKF Extended Kalman Filter for nonlinear dynamic systems - ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P - for nonlinear dynamic system: - x_k+1 = f(x_k) + w_k - y_k = h(x_k) + v_k - where w ~ N(0,Q) meaning w is gaussian noise with covariance Q - v ~ N(0,R) meaning v is gaussian noise with covariance R - Inputs: f: function handle for f(x) - mx_k: "a priori" state estimate - P_k: "a priori" estimated state covariance - h: fanction handle for h(x) - y_kp1: current measurement - Q: process noise covariance - R: measurement noise covariance - args: additional arguments to f(x, *args) - Output: mx_kp1: "a posteriori" state estimate - P_kp1: "a posteriori" state covariance - - Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" - """ - - - xDim = mx_k.size # dimension of the state - mx_kp1 = f(mx_k, *args) # predict next state - A = numerical_jac(f, mx_k, *args) # linearize process model about current state - P_kp1 = dot(dot(A,P_k),A.T) + Q # proprogate variance - my_kp1 = h(mx_kp1) # predict future output - H = numerical_jac(h, mx_kp1) # linearize measurement model about predicted next state - P12 = dot(P_kp1, H.T) # cross covariance - K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain - mx_kp1 = mx_kp1 + dot(K,(y_kp1 - my_kp1)) # state estimate - P_kp1 = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) - - return (mx_kp1, P_kp1) - - - -def numerical_jac(f,x, *args): - """ - Function to compute the numerical jacobian of a vector valued function - using final differences - """ - # numerical gradient and diagonal hessian - y = f(x, *args) - - jac = zeros( (y.size,x.size) ) - eps = 1e-5 - xp = copy(x) - - for i in range(x.size): - xp[i] = x[i] + eps/2.0 - yhi = f(xp, *args) - xp[i] = x[i] - eps/2.0 - ylo = f(xp, *args) - xp[i] = x[i] - jac[:,i] = (yhi - ylo) / eps - return jac diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index a5a366d8..c7082c1e 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -20,8 +20,8 @@ from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 from numpy import pi, cos, sin, eye, array, zeros, diag -from ekf import ekf -from system_models import f_DynBkMdl, h_DynBkMdl +from observers import kinematicLuembergerObserver, ekf +from system_models import f_DynBkMdl, f_DynBkMdl_exact, h_DynBkMdl from tf import transformations from numpy import unwrap @@ -163,7 +163,7 @@ def state_estimation(): # estimation variables for EKF P = eye(6) # initial dynamics coveriance matrix #Q = (q_std**2)*eye(6) # process noise coveriance matrixif est_mode==1: - Q = diag([0.01,0.01,0.1,0.1,1.0,0.01]) # values derived from inspecting P matrix during Kalman filter running + Q = diag([0.01,0.01,0.1,0.0001,1.0,0.001]) # values derived from inspecting P matrix during Kalman filter running if est_mode==1: # use gps, IMU, and encoder R = diag([gps_std,gps_std,psi_std,v_std])**2 @@ -177,6 +177,7 @@ def state_estimation(): rospy.logerr("No estimation mode selected.") running = False + #running = True while not rospy.is_shutdown(): # publish state estimate @@ -201,6 +202,8 @@ def state_estimation(): # apply EKF and get each state estimate (z_EKF,P) = ekf(f_DynBkMdl, z_EKF, P, h_DynBkMdl, y, Q, R, args ) + #print "New Iteration ------" + #print P else: z_EKF[0] = float(x_meas) diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index d1791986..cd3e0dad 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -222,6 +222,26 @@ def f_KinBkMdl(z,u,vhMdl, dt): return array([x_next, y_next, psi_next, v_next]) +def pacejka(a): + B = 0.3#20 + C = 1.25 + mu = 0.234 + m = 1.98 + g = 9.81 + D = mu * m * g/2 + D = D*10 + + C_alpha_f = D*sin(C*arctan(B*a)) + return C_alpha_f + +def f_DynBkMdl_exact(z,u,vhMdl,trMdl,dt): + zNext = z + dtn = dt / 10 + for i in range(0,10): + zNext = f_DynBkMdl(zNext,u,vhMdl,trMdl,dtn) + + return zNext + def f_DynBkMdl(z,u,vhMdl,trMdl,dt): x_I = z[0] y_I = z[1] @@ -244,35 +264,22 @@ def f_DynBkMdl(z,u,vhMdl,trMdl,dt): # ref: Hindiyeh Thesis, p58 a_F = 0 a_R = 0 - if v_x != 0: + if abs(v_x) > 0.1: a_F = arctan((v_y + L_f*psi_dot)/v_x) - d_f a_R = arctan((v_y - L_r*psi_dot)/v_x) - #print "v_x = %f, v_y = %f, psi_dot = %f"%(v_x,v_y,psi_dot) - #print "a_F = %f, a_R = %f"%(a_F, a_R) - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pacejka(TM_param, a_F) - - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pacejka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - a**2) # maximum tire force (resulting from friction and motor acceleration) - FyR = min(FyR_max, max(-FyR_max, FyR_paj)) - - C_alpha_f = 10 - C_alpha_r = 10 - - FyF = -C_alpha_f * a_F - FyR = -C_alpha_r * a_R + print "a_F = %f"%a_F + print "a_R = %f\n"%a_R + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) # compute next state x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) - v_y_next = v_y + dt * (1/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) + v_y_next = v_y + dt * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) psi_next = psi + dt * (psi_dot) - psi_dot_next = psi_dot + dt * (1/I_z*(L_f*FyF*cos(d_f) - L_r*FyR)) + psi_dot_next = psi_dot + dt * (2/I_z*(L_f*FyF - L_r*FyR)) return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next]) From bf3d27e638717174c9ba09b3bd414b93a35279f0 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 28 Sep 2016 19:32:06 -0700 Subject: [PATCH 038/183] Merge with LMPC --- eval_sim/eval_sim.jl | 3 +- workspace/src/barc/src/barc_simulator.jl | 4 +-- .../barc/src/helper/coeffConstraintCost.jl | 28 ++++--------------- workspace/src/barc/src/helper/functions.jl | 4 +-- .../src/barc/src/helper/solveMpcProblem.jl | 8 +++--- workspace/src/barc/src/system_models.py | 8 +++--- 6 files changed, 19 insertions(+), 36 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index e628ae72..96f58404 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -66,7 +66,6 @@ function eval_sim() title("x-y-view") axis("equal") legend(["Real state","GPS meas","estimate"]) - figure() plot(est_dyn.t,est_dyn.z,"-*",est.t,est.z,"--",z.t,z.z,"-") title("Dyn. est. -*, est. --, real -") @@ -80,6 +79,8 @@ function eval_sim() figure() plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*",z.z[:,1],z.z[:,2],"-") + grid(1) + legend(["est","est_dyn","true"]) figure() plot(z.t-t0,z.z[:,5],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 5dc730dd..faa6f67d 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -172,8 +172,8 @@ function main() end # GPS measurements - x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 1*randn()*2) + x = round(z_current[i,1]*100 + 0*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 0*randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl index 2ac5eafc..3fb57d51 100644 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ b/workspace/src/barc/src/helper/coeffConstraintCost.jl @@ -106,29 +106,11 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo # These values are calculated for both old trajectories # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost - for i=1:2 - Qfunction = zeros(N_points,1) - IndexBezierS = idx_s[i] - (i-1)*N_points # IndexBezierS is the index specifying the current position - idx_s_target = find(oldS[:,i].>s_target)[1] - dist_to_s_target = (idx_s_target - IndexBezierS) - dist_to_s_target = dist_to_s_target + 30 - - bQfunction_Vector = zeros(pLength+1,1) - # Select the part needed for the interpolation - #bQfunction_Vector = Qfunction[IndexBezierS:IndexBezierS+pLength] - qLength = min(dist_to_s_target,pLength+1) - #println(bQfunction_Vector) - bQfunction_Vector = zeros(pLength+1,1) - bQfunction_Vector[1:qLength] = (dist_to_s_target:-1:dist_to_s_target-qLength+1)*0.1 - - #bQfunction_Vector = collect((dist_to_s_target:-1:dist_to_s_target-pLength))*0.1 - #println("length = $(length(bQfunction_Vector)), $(pLength+1)") - #println(bQfunction_Vector) - #readline() - - # Compute coefficient for the cost - coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector - + for i=1:2 + dist_to_s_target = oldTraj.oldCost[i] - (idx_s[i]-N_points*(i-1)) # number of iterations from idx_s to s_target + bQfunction_Vector = collect(linspace(dist_to_s_target,dist_to_s_target-1,pLength+1)) # build a vector that starts at the distance and + # decreases in equal steps + coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector # interpolate this vector with the given s end mpcCoeff.coeffCost = coeffCost diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl index c66e2e90..5a6c1286 100644 --- a/workspace/src/barc/src/helper/functions.jl +++ b/workspace/src/barc/src/helper/functions.jl @@ -31,9 +31,9 @@ function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelP mpcParams.N = 10 mpcParams.nz = 4 mpcParams.Q = [0.0,10.0,0.0,0.1] # put weights on ey, epsi and v - mpcParams.Q_term = 1*[0.1,0.01,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) + mpcParams.Q_term = 100*[0.01,0.01,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f - mpcParams.QderivZ = 1.0*[0,0.0,0.1,0] # cost matrix for derivative cost of states + mpcParams.QderivZ = 0.0*[0,0.0,0.1,0] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1,1] # cost matrix for derivative cost of inputs mpcParams.vPathFollowing = 0.6 # reference speed for first lap of path following diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index 9d077c2b..de323330 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -70,20 +70,20 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Terminal constraints (soft), starting from 2nd lap # --------------------------------- if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, constZTerm, 10*(sum{Q_term[j]*(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ + @NLexpression(mdl.mdl, constZTerm, (sum{Q_term[j]*(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3})) elseif lapStatus.currentLap == 2 # if in the 2nd lap - @NLexpression(mdl.mdl, constZTerm, 10*sum{Q_term[j]*(sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3}) + @NLexpression(mdl.mdl, constZTerm, sum{Q_term[j]*(sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3}) end # Terminal cost # --------------------------------- # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line. if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, costZTerm, 1*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ + @NLexpression(mdl.mdl, costZTerm, 10*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1})) elseif lapStatus.currentLap == 2 # if we're in the second second lap - @NLexpression(mdl.mdl, costZTerm, 1*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}) + @NLexpression(mdl.mdl, costZTerm, 10*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}) end # State cost diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index cd3e0dad..6614fb2c 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -317,10 +317,10 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt): psi_next = psi + dt*v/L_b*sin(bta) v_next = v + dt*(a - 0.63*sign(v)*v**2) - x_next_pred = x_next + 0.2*( v*cos(psi + bta) ) - y_next_pred = y_next + 0.2*( v*sin(psi + bta) ) - psi_next_pred = psi_next + 0.2*v/L_b*sin(bta) - v_next_pred = v_next + 0.2*(a - 0.63*sign(v)*v**2) + x_next_pred = x_next + 0.1*( v*cos(psi + bta) ) + y_next_pred = y_next + 0.1*( v*sin(psi + bta) ) + psi_next_pred = psi_next + 0.1*v/L_b*sin(bta) + v_next_pred = v_next + 0.1*(a - 0.63*sign(v)*v**2) return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) From b700ef93f5c91feaf5f101284368aa90941baa95 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 29 Sep 2016 07:27:50 -0700 Subject: [PATCH 039/183] Added predictive dynamic estimator --- eval_sim/eval_sim.jl | 1 + workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/src/barc_simulator.jl | 2 +- .../src/barc/src/helper/solveMpcProblem.jl | 2 +- .../src/barc/src/state_estimation_DynBkMdl.py | 26 +- workspace/src/barc/src/system_models.py | 733 +++++++++--------- 6 files changed, 389 insertions(+), 377 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 96f58404..d2539a59 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -66,6 +66,7 @@ function eval_sim() title("x-y-view") axis("equal") legend(["Real state","GPS meas","estimate"]) + figure() plot(est_dyn.t,est_dyn.z,"-*",est.t,est.z,"--",z.t,z.z,"-") title("Dyn. est. -*, est. --, real -") diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 7cfe7580..e16cbfb5 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -47,7 +47,7 @@ - + diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index faa6f67d..472b6f2f 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -150,7 +150,7 @@ function main() last_updated = dist_traveled FL += 1 FR += 1 - BL += 1 + BL += 0 #no encoder on back left wheel BR += 0 #no encoder on back right wheel enc_data = Encoder(FL, FR, BL, BR) publish(pub_enc, enc_data) #publish a message everytime the encoder counts up diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl index de323330..8fc72c1e 100644 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ b/workspace/src/barc/src/helper/solveMpcProblem.jl @@ -61,7 +61,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # Lane cost # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 100*sum{mdl.z_Ol[i,2]^2*((0.5+0.5*tanh(50*(mdl.z_Ol[i,2]-ey_max))) + (0.5-0.5*tanh(50*(mdl.z_Ol[i,2]+ey_max)))),i=1:N+1}) + @NLexpression(mdl.mdl, laneCost, 10*sum{mdl.z_Ol[i,2]^2*((0.5+0.5*tanh(10*(mdl.z_Ol[i,2]-ey_max))) + (0.5-0.5*tanh(10*(mdl.z_Ol[i,2]+ey_max)))),i=1:N+1}) # Control Input cost # --------------------------------- diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index c7082c1e..8dbc50de 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -143,12 +143,12 @@ def state_estimation(): Ff = rospy.get_param("friction") # get EKF observer properties - q_std = rospy.get_param("state_estimation_dynamic/q_std") # std of process noise - psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise + q_std = rospy.get_param("state_estimation_dynamic/q_std") # std of process noise + psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise v_std = rospy.get_param("state_estimation_dynamic/v_std") - gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - ang_v_std = rospy.get_param("state_estimation_dynamic/ang_v_std") # std of gps measurements - v_x_min = rospy.get_param("state_estimation_dynamic/v_x_min") # minimum velociy before using EKF + gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements + ang_v_std = rospy.get_param("state_estimation_dynamic/ang_v_std") # std of gps measurements + v_x_min = rospy.get_param("state_estimation_dynamic/v_x_min") # minimum velociy before using EKF est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode # set node rate @@ -158,12 +158,12 @@ def state_estimation(): t0 = time.time() # estimation variables for Luemberger observer - z_EKF = zeros(6) + z_EKF = zeros(12) # estimation variables for EKF - P = eye(6) # initial dynamics coveriance matrix + P = eye(12) # initial dynamics coveriance matrix #Q = (q_std**2)*eye(6) # process noise coveriance matrixif est_mode==1: - Q = diag([0.01,0.01,0.1,0.0001,1.0,0.001]) # values derived from inspecting P matrix during Kalman filter running + Q = diag([0.01,0.01,0.1,0.0001,1.0,0.001,0,0,0,0,0,0]) # values derived from inspecting P matrix during Kalman filter running if est_mode==1: # use gps, IMU, and encoder R = diag([gps_std,gps_std,psi_std,v_std])**2 @@ -172,7 +172,7 @@ def state_estimation(): elif est_mode==3: # use gps only R = (gps_std**2)*eye(2) elif est_mode==4: # use gps and angular velocity - R = diag([gps_std,gps_std,ang_v_std])**2 + R = diag([gps_std,gps_std,ang_v_std,v_std])**2 else: rospy.logerr("No estimation mode selected.") @@ -181,17 +181,19 @@ def state_estimation(): while not rospy.is_shutdown(): # publish state estimate - (x,y,v_x,v_y,psi,psi_dot) = z_EKF # note, r = EKF estimate yaw rate + (x,y,v_x,v_y,psi,psi_dot,x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) = z_EKF # note, r = EKF estimate yaw rate # publish information - state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) + #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) + state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) # apply EKF #if v_x_enc > v_x_min: if FxR > 0 or running: running = True # get measurement - y = array([x_meas,y_meas,w_z]) + y = array([x_meas,y_meas,w_z,v_x_enc]) + print "v_x_enc = %f"%v_x_enc # define input u = array([ d_f, FxR ]) diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 6614fb2c..7115b9a3 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -1,362 +1,371 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from numpy import sin, cos, tan, arctan, array, dot -from numpy import sign, argmin, sqrt -import rospy - -# discrete non-linear bicycle model dynamics -def f_2s(z, u, vhMdl, trMdl, dt, v_x): - """ - process model - input: state z at time k, z[k] := [beta[k], r[k]], (i.e. slip angle and yaw rate) - output: state at next time step (k+1) - """ - - # get states / inputs - beta = z[0] - r = z[1] - d_f = u - - # extract parameters - (a,b,m,I_z) = vhMdl - (trMdlFront, trMdlRear) = trMdl - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan(beta + a*r/v_x) - d_f - a_R = arctan(beta - b*r/v_x) - - # compute tire force - FyF = f_pacejka(trMdlFront, a_F) - FyR = f_pacejka(trMdlRear, a_R) - - # compute next state - beta_next = beta + dt*(-r + (1/(m*v_x))*(FyF*cos(d_f)+FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR); - return array([beta_next, r_next]) - -# discrete non-linear bicycle model dynamics -def f_3s(z, u, vhMdl, trMdl, F_ext, dt): - """ - process model - input: state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) - output: state at next time step z[k+1] - """ - - # get states / inputs - v_x = z[0] - v_y = z[1] - r = z[2] - d_f = u[0] - FxR = u[1] - - # extract parameters - (a,b,m,I_z) = vhMdl - (a0, Ff) = F_ext - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # limit force to tire friction circle - if FxR >= mu*Fn: - FxR = mu*Fn - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan((v_y + a*r)/v_x) - d_f - a_R = arctan((v_y - b*r)/v_x) - - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pacejka(TM_param, a_F) - - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pacejka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - FxR**2) - FyR = min(FyR_max, max(-FyR_max, FyR_paj)) - - # compute next state - v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) - v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) - - return array([v_x_next, v_y_next, r_next]) - -# discrete non-linear bicycle model dynamics 6-dof -def f_6s(z, u, vhMdl, trMdl, F_ext, dt): - """ - process model - input: state z at time k, z[k] := [X[k], Y[k], phi[k], v_x[k], v_y[k], r[k]]) - output: state at next time step z[k+1] - """ - - # get states / inputs - X = z[0] - Y = z[1] - phi = z[2] - v_x = z[3] - v_y = z[4] - r = z[5] - - d_f = u[0] - FxR = u[1] - - # extract parameters - (a,b,m,I_z) = vhMdl - (a0, Ff) = F_ext - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # limit force to tire friction circle - if FxR >= mu*Fn: - FxR = mu*Fn - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan((v_y + a*r)/v_x) - d_f - a_R = arctan((v_y - b*r)/v_x) - - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pacejka(TM_param, a_F) - - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pacejka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - FxR**2) - Fy = array([FyR_max, FyR_paj]) - idx = argmin(abs(Fy)) - FyR = Fy[idx] - - # compute next state - X_next = X + dt*(v_x*cos(phi) - v_y*sin(phi)) - Y_next = Y + dt*(v_x*sin(phi) + v_y*cos(phi)) - phi_next = phi + dt*r - v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) - v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) - - return array([X_next, Y_next, phi_next, v_x_next, v_y_next, r_next]) - - - -def h_2s(x): - """ - measurement model - state: z := [beta, r], (i.e. slip angle and yaw rate) - output h := r (yaw rate) - """ - C = array([[0, 1]]) - return dot(C, x) - -def h_3s(x): - """ - measurement model - input := state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) - output := [v_x, r] (yaw rate) - """ - C = array([[1, 0, 0], - [0, 0, 1]]) - return dot(C, x) - - -def f_pacejka(trMdl, alpha): - """ - f_pacejka = d*sin(c*atan(b*alpha)) - - inputs : - * trMdl := tire model, a list or tuple of parameters (b,c,d) - * alpha := tire slip angle [radians] - outputs : - * Fy := lateral force from tire [Newtons] - """ - (b,c,d) = trMdl - return d*sin(c*arctan(b*alpha)) - - -def f_KinBkMdl(z,u,vhMdl, dt): - """ - process model - input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] - output: state at next time step z[k+1] - """ - #c = array([0.5431, 1.2767, 2.1516, -2.4169]) - - # get states / inputs - x = z[0] - y = z[1] - psi = z[2] - v = z[3] - - d_f = u[0] - a = u[1] - - # extract parameters - (L_a, L_b) = vhMdl - - # compute slip angle - bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) - - # compute next state - x_next = x + dt*( v*cos(psi + bta) ) - y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*v/L_b*sin(bta) - v_next = v + dt*(a - 0.63*sign(v)*v**2) - - return array([x_next, y_next, psi_next, v_next]) - -def pacejka(a): - B = 0.3#20 - C = 1.25 - mu = 0.234 - m = 1.98 - g = 9.81 - D = mu * m * g/2 - D = D*10 - - C_alpha_f = D*sin(C*arctan(B*a)) - return C_alpha_f - -def f_DynBkMdl_exact(z,u,vhMdl,trMdl,dt): - zNext = z - dtn = dt / 10 - for i in range(0,10): - zNext = f_DynBkMdl(zNext,u,vhMdl,trMdl,dtn) - - return zNext - -def f_DynBkMdl(z,u,vhMdl,trMdl,dt): - x_I = z[0] - y_I = z[1] - v_x = z[2] - v_y = z[3] - psi = z[4] - psi_dot = z[5] - - d_f = u[0] - a = u[1] - - # extract parameters - (L_f,L_r,m,I_z) = vhMdl - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = 0 - a_R = 0 - if abs(v_x) > 0.1: - a_F = arctan((v_y + L_f*psi_dot)/v_x) - d_f - a_R = arctan((v_y - L_r*psi_dot)/v_x) - - print "a_F = %f"%a_F - print "a_R = %f\n"%a_R - FyF = -pacejka(a_F) - FyR = -pacejka(a_R) - - # compute next state - x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) - y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) - v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) - v_y_next = v_y + dt * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) - psi_next = psi + dt * (psi_dot) - psi_dot_next = psi_dot + dt * (2/I_z*(L_f*FyF - L_r*FyR)) - - return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next]) - -def f_KinBkMdl_predictive(z,u,vhMdl, dt): - """ - process model - input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] - output: state at next time step z[k+1] - """ - #c = array([0.5431, 1.2767, 2.1516, -2.4169]) - - # get states / inputs - x = z[0] - y = z[1] - psi = z[2] - v = z[3] - - x_pred = z[4] - y_pred = z[5] - psi_pred= z[6] - v_pred = z[7] - - d_f = u[0] - a = u[1] - - # extract parameters - (L_a, L_b) = vhMdl - - # compute slip angle - bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) - - # compute next state - x_next = x + dt*( v*cos(psi + bta) ) - y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*v/L_b*sin(bta) - v_next = v + dt*(a - 0.63*sign(v)*v**2) - - x_next_pred = x_next + 0.1*( v*cos(psi + bta) ) - y_next_pred = y_next + 0.1*( v*sin(psi + bta) ) - psi_next_pred = psi_next + 0.1*v/L_b*sin(bta) - v_next_pred = v_next + 0.1*(a - 0.63*sign(v)*v**2) - - return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) - -def h_DynBkMdl(x): - # For GPS only: - C = array([[1, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 1]]) - return dot(C, x) - -def h_KinBkMdl(x): - """ - measurement model - """ - # For GPS, IMU and encoders: - # C = array([[1, 0, 0, 0], - # [0, 1, 0, 0], - # [0, 0, 1, 0], - # [0, 0, 0, 1]]) - - # For GPS only: - C = array([[1, 0, 0, 0], - [0, 1, 0, 0]]) - return dot(C, x) - -def h_KinBkMdl_predictive(x): - """ - measurement model - """ - # For GPS, IMU and encoders: - # C = array([[1, 0, 0, 0], - # [0, 1, 0, 0], - # [0, 0, 1, 0], - # [0, 0, 0, 1]]) - - # For GPS only: - C = array([[1, 0, 0, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0, 0, 0]]) - return dot(C, x) +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +from numpy import sin, cos, tan, arctan, array, dot +from numpy import sign, argmin, sqrt +import rospy + +# discrete non-linear bicycle model dynamics +def f_2s(z, u, vhMdl, trMdl, dt, v_x): + """ + process model + input: state z at time k, z[k] := [beta[k], r[k]], (i.e. slip angle and yaw rate) + output: state at next time step (k+1) + """ + + # get states / inputs + beta = z[0] + r = z[1] + d_f = u + + # extract parameters + (a,b,m,I_z) = vhMdl + (trMdlFront, trMdlRear) = trMdl + + # comptue the front/rear slip [rad/s] + # ref: Hindiyeh Thesis, p58 + a_F = arctan(beta + a*r/v_x) - d_f + a_R = arctan(beta - b*r/v_x) + + # compute tire force + FyF = f_pacejka(trMdlFront, a_F) + FyR = f_pacejka(trMdlRear, a_R) + + # compute next state + beta_next = beta + dt*(-r + (1/(m*v_x))*(FyF*cos(d_f)+FyR)) + r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR); + return array([beta_next, r_next]) + +# discrete non-linear bicycle model dynamics +def f_3s(z, u, vhMdl, trMdl, F_ext, dt): + """ + process model + input: state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) + output: state at next time step z[k+1] + """ + + # get states / inputs + v_x = z[0] + v_y = z[1] + r = z[2] + d_f = u[0] + FxR = u[1] + + # extract parameters + (a,b,m,I_z) = vhMdl + (a0, Ff) = F_ext + (trMdlFront, trMdlRear) = trMdl + (B,C,mu) = trMdlFront + g = 9.81 + Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) + + # limit force to tire friction circle + if FxR >= mu*Fn: + FxR = mu*Fn + + # comptue the front/rear slip [rad/s] + # ref: Hindiyeh Thesis, p58 + a_F = arctan((v_y + a*r)/v_x) - d_f + a_R = arctan((v_y - b*r)/v_x) + + # compute lateral tire force at the front + TM_param = [B, C, mu*Fn] + FyF = -f_pacejka(TM_param, a_F) + + # compute lateral tire force at the rear + # ensure that magnitude of longitudinal/lateral force lie within friction circle + FyR_paj = -f_pacejka(TM_param, a_R) + FyR_max = sqrt((mu*Fn)**2 - FxR**2) + FyR = min(FyR_max, max(-FyR_max, FyR_paj)) + + # compute next state + v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) + v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) + r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) + + return array([v_x_next, v_y_next, r_next]) + +# discrete non-linear bicycle model dynamics 6-dof +def f_6s(z, u, vhMdl, trMdl, F_ext, dt): + """ + process model + input: state z at time k, z[k] := [X[k], Y[k], phi[k], v_x[k], v_y[k], r[k]]) + output: state at next time step z[k+1] + """ + + # get states / inputs + X = z[0] + Y = z[1] + phi = z[2] + v_x = z[3] + v_y = z[4] + r = z[5] + + d_f = u[0] + FxR = u[1] + + # extract parameters + (a,b,m,I_z) = vhMdl + (a0, Ff) = F_ext + (trMdlFront, trMdlRear) = trMdl + (B,C,mu) = trMdlFront + g = 9.81 + Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) + + # limit force to tire friction circle + if FxR >= mu*Fn: + FxR = mu*Fn + + # comptue the front/rear slip [rad/s] + # ref: Hindiyeh Thesis, p58 + a_F = arctan((v_y + a*r)/v_x) - d_f + a_R = arctan((v_y - b*r)/v_x) + + # compute lateral tire force at the front + TM_param = [B, C, mu*Fn] + FyF = -f_pacejka(TM_param, a_F) + + # compute lateral tire force at the rear + # ensure that magnitude of longitudinal/lateral force lie within friction circle + FyR_paj = -f_pacejka(TM_param, a_R) + FyR_max = sqrt((mu*Fn)**2 - FxR**2) + Fy = array([FyR_max, FyR_paj]) + idx = argmin(abs(Fy)) + FyR = Fy[idx] + + # compute next state + X_next = X + dt*(v_x*cos(phi) - v_y*sin(phi)) + Y_next = Y + dt*(v_x*sin(phi) + v_y*cos(phi)) + phi_next = phi + dt*r + v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) + v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) + r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) + + return array([X_next, Y_next, phi_next, v_x_next, v_y_next, r_next]) + +def h_2s(x): + """ + measurement model + state: z := [beta, r], (i.e. slip angle and yaw rate) + output h := r (yaw rate) + """ + C = array([[0, 1]]) + return dot(C, x) + +def h_3s(x): + """ + measurement model + input := state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) + output := [v_x, r] (yaw rate) + """ + C = array([[1, 0, 0], + [0, 0, 1]]) + return dot(C, x) + +def f_pacejka(trMdl, alpha): + """ + f_pacejka = d*sin(c*atan(b*alpha)) + + inputs : + * trMdl := tire model, a list or tuple of parameters (b,c,d) + * alpha := tire slip angle [radians] + outputs : + * Fy := lateral force from tire [Newtons] + """ + (b,c,d) = trMdl + return d*sin(c*arctan(b*alpha)) + + +def f_KinBkMdl(z,u,vhMdl, dt): + """ + process model + input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] + output: state at next time step z[k+1] + """ + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) + + # get states / inputs + x = z[0] + y = z[1] + psi = z[2] + v = z[3] + + d_f = u[0] + a = u[1] + + # extract parameters + (L_a, L_b) = vhMdl + + # compute slip angle + bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + + # compute next state + x_next = x + dt*( v*cos(psi + bta) ) + y_next = y + dt*( v*sin(psi + bta) ) + psi_next = psi + dt*v/L_b*sin(bta) + v_next = v + dt*(a - 0.63*sign(v)*v**2) + + return array([x_next, y_next, psi_next, v_next]) + +def pacejka(a): + B = 0.3#20 + C = 1.25 + mu = 0.234 + m = 1.98 + g = 9.81 + D = mu * m * g/2 + D = D*10 + + C_alpha_f = D*sin(C*arctan(B*a)) + return C_alpha_f + +def f_DynBkMdl_exact(z,u,vhMdl,trMdl,dt): + zNext = z + dtn = dt / 10 + for i in range(0,10): + zNext = f_DynBkMdl(zNext,u,vhMdl,trMdl,dtn) + + return zNext + +def f_DynBkMdl(z,u,vhMdl,trMdl,dt): + x_I = z[0] + y_I = z[1] + v_x = z[2] + v_y = z[3] + psi = z[4] + psi_dot = z[5] + x_I_pred = z[6] + y_I_pred = z[7] + v_x_pred = z[8] + v_y_pred = z[9] + psi_pred = z[10] + psi_dot_pred = z[11] + + d_f = u[0] + a = u[1] + + dt_pred = 0.15 + + # extract parameters + (L_f,L_r,m,I_z) = vhMdl + (trMdlFront, trMdlRear) = trMdl + (B,C,mu) = trMdlFront + g = 9.81 + Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) + + # comptue the front/rear slip [rad/s] + # ref: Hindiyeh Thesis, p58 + a_F = 0 + a_R = 0 + if abs(v_x) > 0.1: + a_F = arctan((v_y + L_f*psi_dot)/v_x) - d_f + a_R = arctan((v_y - L_r*psi_dot)/v_x) + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + # compute next state + x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) + y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) + v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) + v_y_next = v_y + dt * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) + psi_next = psi + dt * (psi_dot) + psi_dot_next = psi_dot + dt * (2/I_z*(L_f*FyF - L_r*FyR)) + + x_I_next_pred = x_I + dt_pred * (cos(psi)*v_x - sin(psi)*v_y) + y_I_next_pred = y_I + dt_pred * (sin(psi)*v_x + cos(psi)*v_y) + v_x_next_pred = v_x + dt_pred * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) + v_y_next_pred = v_y + dt_pred * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) + psi_next_pred = psi + dt_pred * (psi_dot) + psi_dot_next_pred = psi_dot + dt_pred * (2/I_z*(L_f*FyF - L_r*FyR)) + + return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next,x_I_next_pred,y_I_next_pred,v_x_next_pred,v_y_next_pred,psi_next_pred,psi_dot_next_pred]) + +def f_KinBkMdl_predictive(z,u,vhMdl, dt): + """ + process model + input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] + output: state at next time step z[k+1] + """ + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) + + # get states / inputs + x = z[0] + y = z[1] + psi = z[2] + v = z[3] + + x_pred = z[4] + y_pred = z[5] + psi_pred= z[6] + v_pred = z[7] + + d_f = u[0] + a = u[1] + + # extract parameters + (L_a, L_b) = vhMdl + + # compute slip angle + bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + + # compute next state + x_next = x + dt*( v*cos(psi + bta) ) + y_next = y + dt*( v*sin(psi + bta) ) + psi_next = psi + dt*v/L_b*sin(bta) + v_next = v + dt*(a - 0.63*sign(v)*v**2) + + x_next_pred = x_next + 0.1*( v*cos(psi + bta) ) + y_next_pred = y_next + 0.1*( v*sin(psi + bta) ) + psi_next_pred = psi_next + 0.1*v/L_b*sin(bta) + v_next_pred = v_next + 0.1*(a - 0.63*sign(v)*v**2) + + return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) + +def h_DynBkMdl(x): + # For GPS only: + C = array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]]) + return dot(C, x) + +def h_KinBkMdl(x): + """ + measurement model + """ + # For GPS, IMU and encoders: + # C = array([[1, 0, 0, 0], + # [0, 1, 0, 0], + # [0, 0, 1, 0], + # [0, 0, 0, 1]]) + # For GPS only: + C = array([[1, 0, 0, 0], + [0, 1, 0, 0]]) + return dot(C, x) + +def h_KinBkMdl_predictive(x): + """ + measurement model + """ + # For GPS, IMU and encoders: + # C = array([[1, 0, 0, 0], + # [0, 1, 0, 0], + # [0, 0, 1, 0], + # [0, 0, 0, 1]]) + # For GPS only: + C = array([[1, 0, 0, 0, 0, 0, 0, 0], + [0, 1, 0, 0, 0, 0, 0, 0]]) + return dot(C, x) From 1bdff90387703815c48c1c3b74ad67d0b48f9a5f Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 5 Oct 2016 17:40:14 -0700 Subject: [PATCH 040/183] Deleted files and replaced them with the submodule --- .gitmodules | 3 + workspace/src/barc/src/LMPC_lib | 1 + workspace/src/barc/src/helper/README.txt | 39 ---- workspace/src/barc/src/helper/classes.jl | 112 ------------ .../barc/src/helper/coeffConstraintCost.jl | 120 ------------- .../src/barc/src/helper/computeCostLap.jl | 9 - workspace/src/barc/src/helper/functions.jl | 166 ------------------ .../src/barc/src/helper/solveMpcProblem.jl | 159 ----------------- 8 files changed, 4 insertions(+), 605 deletions(-) create mode 100644 .gitmodules create mode 160000 workspace/src/barc/src/LMPC_lib delete mode 100644 workspace/src/barc/src/helper/README.txt delete mode 100644 workspace/src/barc/src/helper/classes.jl delete mode 100644 workspace/src/barc/src/helper/coeffConstraintCost.jl delete mode 100644 workspace/src/barc/src/helper/computeCostLap.jl delete mode 100644 workspace/src/barc/src/helper/functions.jl delete mode 100644 workspace/src/barc/src/helper/solveMpcProblem.jl diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..b30135c1 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "workspace/src/barc/src/LMPC_lib"] + path = workspace/src/barc/src/LMPC_lib + url = https://github.com/maxb91/LMPC_lib diff --git a/workspace/src/barc/src/LMPC_lib b/workspace/src/barc/src/LMPC_lib new file mode 160000 index 00000000..edbf8b7f --- /dev/null +++ b/workspace/src/barc/src/LMPC_lib @@ -0,0 +1 @@ +Subproject commit edbf8b7ff8f04976f8bb311ba0886dd30235554b diff --git a/workspace/src/barc/src/helper/README.txt b/workspace/src/barc/src/helper/README.txt deleted file mode 100644 index c1a2e0c7..00000000 --- a/workspace/src/barc/src/helper/README.txt +++ /dev/null @@ -1,39 +0,0 @@ -This file helps to explain the structure of the LMPC control. - - -1. The estimator and Localization node -====================================== -The estimator estimates the current state (x, y, psi, v) by using a Kalman filter on the sensor data. -The localization node maps this estimated data on the given track and calculates following values: -s_start = distance along the track, starting from the start/finish line to where a polynomial approximation of the track's curvature starts -s = distance along the track, starting at s_start, to the current position of the car -eY = distance of the car to the track (perpendicular to track) -ePsi = difference of the car's heading and the current reference heading (tangent to the track) -v = current velocity -The track's shape has to be defined in Localization_node.py. - -2. The LMPC node -================ -The LMPC node receives the position info (s_start, s, eY, ePsi, v) and first calculates the mpc coefficients (they are part of the "learning" part and account for previous laps). The coefficients approximate polynomials that are used in the MPC control, in the interval closest_s to closest_s + 2*N. -These coefficients are then sent to the solveMpcProblem.jl function which calculates the new optimal input. - - -Further info -============ -Confusion with different s's: -s_target = length of the track - -Localization node: ------------------- -It returns s_start in the interval of 0 < s_start < s_target and s in the interval 0 < s < nPoints*ds (with nPoints = number of nodes along the track which are used to calculate the approximate polynomial and ds = distance between nodes). -s, s_start and the polynomial approximation coefficients are received by the LMPC_node and written in the state *and* posInfo. -Important: The polynomial coefficients are only valid in the interval 0 < s < nPoints*ds, starting from s_start !!! - -coeffConstraintCost: --------------------- -This function calculates coefficients which approximate the previous trajectory around the current position s_total = s_start + s. The approximated polynomials are valid in the range s_total < s_poly < s_total + 2*N (with N = prediction horizon of the MPC). - -solveMpcProblem: ----------------- -This is the MPC solver, it calculates the optimal input controls by predicting the state evolution, starting at the current state. It uses the states [s, ey, epsi, v] and the coefficients from the Localization node (track coefficients) for calculating the evolution. Therefore, it is important that for the state evolution the relative value s is used! -However, there's a second part to the MPC, which calculates terminal constraints and cost (heart of the learning part). \ No newline at end of file diff --git a/workspace/src/barc/src/helper/classes.jl b/workspace/src/barc/src/helper/classes.jl deleted file mode 100644 index 336cf637..00000000 --- a/workspace/src/barc/src/helper/classes.jl +++ /dev/null @@ -1,112 +0,0 @@ -# VARIOUS TYPES FOR CALCULATIONS - -type LapStatus - currentLap::Int64 # current lap number - currentIt::Int64 # current iteration in current lap -end - -# Structure of coeffConst: -# 1st dimension specifies the state (1 = eY, 2 = ePsi, 3 = v) -# 2nd dimension is the polynomial coefficient -# 3rd dimension is not used -# 4th dimension specifies one of the two lap numbers between which are iterated - -type MpcCoeff # coefficients for trajectory approximation - coeffCost::Array{Float64} - coeffConst::Array{Float64} - order::Int64 - pLength::Int64 # small values here may lead to numerical problems since the functions are only approximated in a short horizon - # "small" values are about 2*N, good values about 4*N - # numerical problems occur at the edges (s=0, when v is almost 0 and s does not change fast and at s=s_target) - MpcCoeff(coeffCost=Float64[], coeffConst=Float64[], order=4, pLength=0) = new(coeffCost, coeffConst, order, pLength) -end - -type OldTrajectory # information about previous trajectories - oldTraj::Array{Float64} - oldInput::Array{Float64} - oldCost::Array{Int64} - OldTrajectory(oldTraj=Float64[],oldInput=Float64[],oldCost=Float64[]) = new(oldTraj,oldInput,oldCost) -end - -type MpcParams # parameters for MPC solver - N::Int64 - nz::Int64 - OrderCostCons::Int64 - Q::Array{Float64,1} - Q_term::Array{Float64,1} - R::Array{Float64,1} - vPathFollowing::Float64 - QderivZ::Array{Float64,1} - QderivU::Array{Float64,1} - MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[]) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing) -end - -type PosInfo # current position information - s_start::Float64 - s::Float64 - s_target::Float64 - PosInfo(s_start=0,s=0,s_target=0) = new(s_start,s,s_target) -end - -type MpcSol # MPC solution output - a_x::Float64 - d_f::Float64 - solverStatus::Symbol - u::Array{Float64} - z::Array{Float64} - cost::Array{Float64} - MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) -end - -type TrackCoeff # coefficients of track - coeffAngle::Array{Float64,1} - coeffCurvature::Array{Float64,1} - nPolyCurvature::Int64 # order of the interpolation polynom - width::Float64 # lane width -> is used in cost function as soft constraints (to stay on track) - TrackCoeff(coeffAngle=Float64[],coeffCurvature=Float64[],nPolyCurvature=4,width=1.0) = new(coeffAngle,coeffCurvature,nPolyCurvature) -end - -type ModelParams - l_A::Float64 - l_B::Float64 - dt::Float64 - u_lb::Array{Float64} # lower bounds for u - u_ub::Array{Float64} # upper bounds - z_lb::Array{Float64} - z_ub::Array{Float64} - c0::Array{Float64} - ModelParams(l_A=0.25,l_B=0.25,dt=0.1,u_lb=Float64[],u_ub=Float64[],z_lb=Float64[],z_ub=Float64[],c0=Float64[]) = new(l_A,l_B,dt,u_lb,u_ub,z_lb,z_ub,c0) -end - -type MpcModel - mdl::JuMP.Model - - z0::Array{JuMP.NonlinearParameter,1} - coeff::Array{JuMP.NonlinearParameter,1} - - z_Ol::Array{JuMP.Variable,2} - u_Ol::Array{JuMP.Variable,2} - ParInt::Array{JuMP.Variable,1} - - dsdt::Array{JuMP.NonlinearExpression,1} - bta::Array{JuMP.NonlinearExpression,1} - c::Array{JuMP.NonlinearExpression,1} - - MpcModel(mdl=JuMP.Model(), - z0=@NLparameter(mdl,z0[i=1:4]==0), - coeff=@NLparameter(mdl,coeff[i=1:5]==0), - z_Ol=@variable(mdl,[1:4,1:10]), - u_Ol=@variable(mdl,[1:2,1:9]), - ParInt=@variable(mdl,[1:1]), - dsdt=@NLexpression(mdl,dsdt[1:10],0), - bta=@NLexpression(mdl,bta[1:10],0), - c=@NLexpression(mdl,c[1:10],0)) = new(mdl, - z0, - coeff, - z_Ol, - u_Ol, - ParInt, - dsdt, - bta, - c) -end diff --git a/workspace/src/barc/src/helper/coeffConstraintCost.jl b/workspace/src/barc/src/helper/coeffConstraintCost.jl deleted file mode 100644 index 3fb57d51..00000000 --- a/workspace/src/barc/src/helper/coeffConstraintCost.jl +++ /dev/null @@ -1,120 +0,0 @@ -# This function evaluates and returns the coefficients for constraints and cost which are used later in the MPC formulation -# Inputs are: -# oldTraj -> contains information about previous trajectories and Inputs -# mpcCoeff -> contains information about previous coefficient results -# posInfo -> contains information about the car's current position along the track -# mpcParams -> contains information about the MPC formulation (e.g. Q, R) -# stateIn -> current state of the car -# inputIn -> last input to the system - -# structure of oldTrajectory: 1st dimension = state number, 2nd dimension = step number (time equiv.), 3rd dimennsion = lap number - -function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams) - # this computes the coefficients for the cost and constraints - - # Outputs: - # coeffConst - # coeffCost - - # Read Inputs - s_start = posInfo.s_start - s = posInfo.s - s_target = posInfo.s_target - - - # Parameters - N = mpcParams.N - nz = mpcParams.nz - R = mpcParams.R - Order = mpcCoeff.order # interpolation order for cost and constraints - - pLength = mpcCoeff.pLength # interpolation length for polynomials - - coeffCost = zeros(Order+1,2) # polynomial coefficients for cost - coeffConst = zeros(Order+1,2,3) # nz-1 beacuse no coeff for s - - # Select the old data - oldS = oldTraj.oldTraj[:,1,:]::Array{Float64,3} - oldeY = oldTraj.oldTraj[:,2,:]::Array{Float64,3} - oldePsi = oldTraj.oldTraj[:,3,:]::Array{Float64,3} - oldV = oldTraj.oldTraj[:,4,:]::Array{Float64,3} - - N_points = size(oldTraj.oldTraj,1) # second dimension = length - - s_total::Float64 # initialize - DistS::Array{Float64} # initialize - idx_s::Array{Int64} # initialize - idx_s_target = 0 - dist_to_s_target = 0 - qLength = 0 - vec_range::Tuple{UnitRange{Int64},UnitRange{Int64}} - bS_Vector::Array{Float64} - s_forinterpy::Array{Float64} - - # Compute the total s (current position along track) - s_total = (s_start + s) % s_target - - # Compute the index - DistS = ( s_total - oldS ).^2 - - idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! - - vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) - - # Create the vectors used for the interpolation - bS_Vector = zeros(pLength+1,2) - for i=1:pLength+1 - bS_Vector[i,1] = oldS[vec_range[1][i]] - bS_Vector[i,2] = oldS[vec_range[2][i]] - end - # bS_Vector = cat(2, oldS[vec_range[1]], oldS[vec_range[2]]) - - # println("************************************** COEFFICIENTS **************************************") - # println("idx_s[1] = $(idx_s[1]), idx_s[2] = $(idx_s[2])") - # println("s_total = $s_total") - # println("bS_Vector[1] = $(bS_Vector[:,:,1]')") - # These matrices (above) contain two vectors each (for both old trajectories), stored in the 3rd dimension - - # The states are parametrized with resprect to the curvilinear abscissa, - # so we select the point used for the interpolation. Need to subtract an - # offset to be coherent with the MPC formulation - s_forinterpy = bS_Vector - s_start - if s_total - s_start < 0 - s_forinterpy += s_target - end - # println("s_forinterpy[:,1,1]' = $(s_forinterpy[:,1,1]')") - # Create the Matrices for the interpolation - MatrixInterp = zeros(pLength+1,Order+1,2) - - for k = 0:Order - MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k - end - - # Compute the coefficients - coeffConst = zeros(Order+1,2,3) - for i=1:2 - coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] - coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] - coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldV[vec_range[i]] - end - - # Finished with calculating the constraint coefficients - - # Now compute the final cost coefficients - - # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value - # These values are calculated for both old trajectories - # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line - # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost - for i=1:2 - dist_to_s_target = oldTraj.oldCost[i] - (idx_s[i]-N_points*(i-1)) # number of iterations from idx_s to s_target - bQfunction_Vector = collect(linspace(dist_to_s_target,dist_to_s_target-1,pLength+1)) # build a vector that starts at the distance and - # decreases in equal steps - coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector # interpolate this vector with the given s - end - - mpcCoeff.coeffCost = coeffCost - mpcCoeff.coeffConst = coeffConst - - nothing -end diff --git a/workspace/src/barc/src/helper/computeCostLap.jl b/workspace/src/barc/src/helper/computeCostLap.jl deleted file mode 100644 index 85d9325a..00000000 --- a/workspace/src/barc/src/helper/computeCostLap.jl +++ /dev/null @@ -1,9 +0,0 @@ -function computeCostLap(z,s_target) - - f = find(z[:,1].>=s_target) - cost = 100000 - if size(f,1)>0 - cost = f[1] - end - return cost -end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/functions.jl b/workspace/src/barc/src/helper/functions.jl deleted file mode 100644 index 5a6c1286..00000000 --- a/workspace/src/barc/src/helper/functions.jl +++ /dev/null @@ -1,166 +0,0 @@ -function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{Float64},lapStatus::LapStatus,buffersize::Int64,dt::Float64) - - i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr_export = zeros(buffersize,4) - uCurr_export = zeros(buffersize,2) - zCurr_export = cat(1,zCurr[1:i-1,:], [zCurr[i-1,1]+collect(1:buffersize-i+1)*dt*zCurr[i-1,4] ones(buffersize-i+1,1)*zCurr[i-1,2:4]]) - uCurr_export = cat(1,uCurr[1:i-1,:], zeros(buffersize-i+1,2)) - costLap = lapStatus.currentIt # the cost of the current lap is the time it took to reach the finish line - - # Save all data in oldTrajectory: - if lapStatus.currentLap == 1 # if it's the first lap - oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything - oldTraj.oldInput[:,:,1] = uCurr_export - oldTraj.oldTraj[:,:,2] = zCurr_export - oldTraj.oldInput[:,:,2] = uCurr_export - oldTraj.oldCost = [costLap,costLap] - else # idea: always copy the new trajectory in the first array! - if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second - oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second - oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input - oldTraj.oldCost[2] = oldTraj.oldCost[1] - end - oldTraj.oldTraj[:,:,1] = zCurr_export # ... and write the new traj in the first - oldTraj.oldInput[:,:,1] = uCurr_export - oldTraj.oldCost[1] = costLap - end -end - -function InitializeParameters(mpcParams::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, - posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) - mpcParams.N = 10 - mpcParams.nz = 4 - mpcParams.Q = [0.0,10.0,0.0,0.1] # put weights on ey, epsi and v - mpcParams.Q_term = 100*[0.01,0.01,1.0] # weights for terminal constraints (LMPC, for e_y, e_psi, and v) - mpcParams.R = 0*[1.0,1.0] # put weights on a and d_f - mpcParams.QderivZ = 0.0*[0,0.0,0.1,0] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[1,1] # cost matrix for derivative cost of inputs - mpcParams.vPathFollowing = 0.6 # reference speed for first lap of path following - - trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation - trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.width = 0.6 # width of the track (0.5m) - - modelParams.u_lb = ones(mpcParams.N,1) * [0.0 -pi/6] # lower bounds on steering - modelParams.u_ub = ones(mpcParams.N,1) * [1.2 pi/6] # upper bounds - modelParams.z_lb = ones(mpcParams.N+1,1)*[-Inf -Inf -Inf -0.1] # lower bounds on states - modelParams.z_ub = ones(mpcParams.N+1,1)*[ Inf Inf Inf 2.0] # upper bounds - #modelParams.c0 = [0.5431, 1.2767, 2.1516, -2.4169] # BARC-specific parameters (measured) - modelParams.c0 = [1, 0.63, 1, 0] # BARC-specific parameters (measured) - modelParams.l_A = 0.125 - modelParams.l_B = 0.125 - modelParams.dt = 0.1 - - posInfo.s_start = 0.0 - posInfo.s_target = 36.84#31.62#25.62#29.491949#13.20#10.281192 - - oldTraj.oldTraj = zeros(buffersize,4,2) - oldTraj.oldTraj[:,1,1] = 1:buffersize - oldTraj.oldTraj[:,1,2] = 1:buffersize - oldTraj.oldInput = zeros(buffersize,2,2) - oldTraj.oldCost = 100*ones(Int64,2) # dummies for initialization - - mpcCoeff.order = 5 - mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) - mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) # nz-1 because no coeff for s - mpcCoeff.pLength = 4*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon - - lapStatus.currentLap = 1 # initialize lap number - lapStatus.currentIt = 0 # current iteration in lap -end - -function InitializeModel(m::MpcModel,mpcParams::MpcParams,modelParams::ModelParams,trackCoeff::TrackCoeff,z_Init::Array{Float64,1}) - - dt = modelParams.dt - L_a = modelParams.l_A - L_b = modelParams.l_B - c0 = modelParams.c0 - u_lb = modelParams.u_lb - u_ub = modelParams.u_ub - z_lb = modelParams.z_lb - z_ub = modelParams.z_ub - - N = mpcParams.N - - n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation - - m.mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.05))#,linear_solver="ma57",print_user_options="yes")) - - @variable( m.mdl, m.z_Ol[1:(N+1),1:4]) # z = s, ey, epsi, v - @variable( m.mdl, m.u_Ol[1:N,1:2]) - @variable( m.mdl, 0 <= m.ParInt[1:1] <= 1) - - for i=1:2 # I don't know why but somehow the short method returns errors sometimes - for j=1:N - setlowerbound(m.u_Ol[j,i], modelParams.u_lb[j,i]) - setupperbound(m.u_Ol[j,i], modelParams.u_ub[j,i]) - end - end - for i=1:4 - for j=1:N+1 - setlowerbound(m.z_Ol[j,i], modelParams.z_lb[j,i]) - setupperbound(m.z_Ol[j,i], modelParams.z_ub[j,i]) - end - end - #@variable( m.mdl, 1 >= m.ParInt >= 0 ) - - @NLparameter(m.mdl, m.z0[i=1:4] == z_Init[i]) - @NLconstraint(m.mdl, [i=1:4], m.z_Ol[1,i] == m.z0[i]) - - @NLparameter(m.mdl, m.coeff[i=1:n_poly_curv+1] == trackCoeff.coeffCurvature[i]); - - #@NLexpression(m.mdl, m.c[i = 1:N], m.coeff[1]*m.z_Ol[1,i]^4+m.coeff[2]*m.z_Ol[1,i]^3+m.coeff[3]*m.z_Ol[1,i]^2+m.coeff[4]*m.z_Ol[1,i]+m.coeff[5]) - @NLexpression(m.mdl, m.c[i = 1:N], sum{m.coeff[j]*m.z_Ol[i,1]^(n_poly_curv-j+1),j=1:n_poly_curv} + m.coeff[n_poly_curv+1]) - @NLexpression(m.mdl, m.bta[i = 1:N], atan( L_a / (L_a + L_b) * ( m.u_Ol[i,2] ) ) ) - @NLexpression(m.mdl, m.dsdt[i = 1:N], m.z_Ol[i,4]*cos(m.z_Ol[i,3]+m.bta[i])/(1-m.z_Ol[i,2]*m.c[i])) - - # System dynamics - for i=1:N - @NLconstraint(m.mdl, m.z_Ol[i+1,1] == m.z_Ol[i,1] + dt*m.dsdt[i] ) # s - @NLconstraint(m.mdl, m.z_Ol[i+1,2] == m.z_Ol[i,2] + dt*m.z_Ol[i,4]*sin(m.z_Ol[i,3]+m.bta[i]) ) # ey - @NLconstraint(m.mdl, m.z_Ol[i+1,3] == m.z_Ol[i,3] + dt*(m.z_Ol[i,4]/L_a*sin(m.bta[i])-m.dsdt[i]*m.c[i]) ) # epsi - @NLconstraint(m.mdl, m.z_Ol[i+1,4] == m.z_Ol[i,4] + dt*(m.u_Ol[i,1] - 0.63*abs(m.z_Ol[i,4]) * m.z_Ol[i,4])) # v - end - -end - -function simModel(z::Array{Float64},u::Array{Float64},modelParams::ModelParams,trackCoeff::TrackCoeff) - - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - println("SIMULATING: u = $u, z0 = $z") - - dt = modelParams.dt/10 - l_A = modelParams.l_A - l_B = modelParams.l_B - - zNext = copy(z) - for i=1:10 - s = zNext[1] - c = 0 - for j=trackCoeff.nPolyCurvature:-1:0 - c += s^j * trackCoeff.coeffCurvature[trackCoeff.nPolyCurvature+1-j] - end - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - dsdt = zNext[4] *cos(zNext[3]+bta)/(1-zNext[2]*c) - - zNext[1] = zNext[1] + dt*dsdt # s - zNext[2] = zNext[2] + dt*(zNext[4]*sin(zNext[3] + bta)) # y - zNext[3] = zNext[3] + dt*(zNext[4]/l_A*sin(bta)-dsdt*c) # psi - zNext[4] = zNext[4] + dt*(u[1] - 0.63 * zNext[4]^2 * sign(zNext[4])) # v - end - - return zNext -end - -function extendOldTraj(oldTraj::OldTrajectory,posInfo::PosInfo,zCurr::Array{Float64,2}) - #println(size(zCurr[1:21,:])) - #println(size(oldTraj.oldTraj[oldTraj.oldCost[1]:oldTraj.oldCost[1]+20,1:4,1])) - for i=1:4 - for j=1:50 - oldTraj.oldTraj[oldTraj.oldCost[1]+j-1,i,1] = zCurr[j,i] - end - end - oldTraj.oldTraj[oldTraj.oldCost[1]:oldTraj.oldCost[1]+49,1,1] += posInfo.s_target -end \ No newline at end of file diff --git a/workspace/src/barc/src/helper/solveMpcProblem.jl b/workspace/src/barc/src/helper/solveMpcProblem.jl deleted file mode 100644 index 8fc72c1e..00000000 --- a/workspace/src/barc/src/helper/solveMpcProblem.jl +++ /dev/null @@ -1,159 +0,0 @@ -# Variable definitions -# mdl.z_Ol[i,j] = z_OpenLoop, open loop prediction of the state, i = state, j = step - -# States: -# i = 1 -> s -# i = 2 -> ey -# i = 3 -> epsi -# i = 4 -> v - -function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uCurr::Array{Float64}) - - # Load Parameters - coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} - N = mpcParams.N - Q = mpcParams.Q - Q_term = mpcParams.Q_term - R = mpcParams.R - coeffTermCost = mpcCoeff.coeffCost::Array{Float64,2} - coeffTermConst = mpcCoeff.coeffConst::Array{Float64,3} - order = mpcCoeff.order # polynomial order of terminal constraints and cost approximation - s_start = posInfo.s_start - s_target = posInfo.s_target - ey_max = trackCoeff.width/2 - - QderivZ = mpcParams.QderivZ::Array{Float64,1} - QderivU = mpcParams.QderivU::Array{Float64,1} - - v_ref = mpcParams.vPathFollowing - - sol_u::Array{Float64,2} - sol_z::Array{Float64,2} - - # println("************************************** MPC SOLVER **************************************") - # println("zCurr = $(zCurr')") - # println("s_start = $s_start") - # println("s_target = $s_target") - # println("s_total = $((zCurr[1]+s_start)%s_target)") - - # Create function-specific parameters - z_Ref::Array{Float64,2} - z_Ref = cat(2,s_target*ones(N+1,1),zeros(N+1,2),v_ref*ones(N+1,1)) # Reference trajectory: path following -> stay on line and keep constant velocity - u_Ref = zeros(N,2) - - # Update current initial condition - setvalue(mdl.z0,zCurr) - # Update curvature - setvalue(mdl.coeff,coeffCurvature) - println("z0 = $(getvalue(mdl.z0))") - println("coeffCurvature = $(getvalue(mdl.coeff))") - - @NLexpression(mdl.mdl, costZ, 0) - @NLexpression(mdl.mdl, costZTerm, 0) - @NLexpression(mdl.mdl, constZTerm, 0) - @NLexpression(mdl.mdl, derivCost, 0) - @NLexpression(mdl.mdl, laneCost, 0) - - # Derivative cost - # --------------------------------- - @NLexpression(mdl.mdl, derivCost, sum{QderivZ[j]*((zCurr[j]-mdl.z_Ol[1,j])^2+sum{(mdl.z_Ol[i,j]-mdl.z_Ol[i+1,j])^2,i=1:N}),j=1:4} + - sum{QderivU[j]*((uCurr[j]-mdl.u_Ol[1,j])^2+sum{(mdl.u_Ol[i,j]-mdl.u_Ol[i+1,j])^2,i=1:N-1}),j=1:2}) - - # Lane cost - # --------------------------------- - @NLexpression(mdl.mdl, laneCost, 10*sum{mdl.z_Ol[i,2]^2*((0.5+0.5*tanh(10*(mdl.z_Ol[i,2]-ey_max))) + (0.5-0.5*tanh(10*(mdl.z_Ol[i,2]+ey_max)))),i=1:N+1}) - - # Control Input cost - # --------------------------------- - @NLexpression(mdl.mdl, controlCost, 0.5*sum{R[j]*sum{(mdl.u_Ol[i,j]-u_Ref[i,j])^2,i=1:N},j=1:2}) - - # Terminal constraints (soft), starting from 2nd lap - # --------------------------------- - if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, constZTerm, (sum{Q_term[j]*(mdl.ParInt[1]*sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ - (1-mdl.ParInt[1])*sum{coeffTermConst[i,2,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3})) - elseif lapStatus.currentLap == 2 # if in the 2nd lap - @NLexpression(mdl.mdl, constZTerm, sum{Q_term[j]*(sum{coeffTermConst[i,1,j]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}-mdl.z_Ol[N+1,j+1])^2,j=1:3}) - end - - # Terminal cost - # --------------------------------- - # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line. - if lapStatus.currentLap > 2 # if at least in the 3rd lap - @NLexpression(mdl.mdl, costZTerm, 10*(mdl.ParInt[1]*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}+ - (1-mdl.ParInt[1])*sum{coeffTermCost[i,2]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1})) - elseif lapStatus.currentLap == 2 # if we're in the second second lap - @NLexpression(mdl.mdl, costZTerm, 10*sum{coeffTermCost[i,1]*mdl.z_Ol[N+1,1]^(order+1-i),i=1:order+1}) - end - - # State cost - # --------------------------------- - if lapStatus.currentLap <= 1 # if we're in the first lap, just do path following - @NLexpression(mdl.mdl, costZ, 0.5*sum{Q[i]*sum{(mdl.z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:4}) # Follow trajectory - - else # if we're in another lap, put cost on z (actually should put cost only on z before finishing the lap) - #@NLexpression(mdl.mdl, costZ_h, 0) # zero state cost after crossing the finish line - #@NLexpression(mdl.mdl, costZ, 1 + (costZ_h-1) * (0.5+0.5*tanh(50*(mdl.z_Ol[1,N+1]+s_start-s_target)))) - @NLexpression(mdl.mdl, costZ, 1) - end - - @NLobjective(mdl.mdl, Min, costZ + costZTerm + constZTerm + derivCost + controlCost + laneCost) - - #println("Model formulation:") - #println(mdl.mdl) - # Solve Problem and return solution - sol_status = solve(mdl.mdl) - sol_u = getvalue(mdl.u_Ol) - sol_z = getvalue(mdl.z_Ol) - println("Predicting until z = $(sol_z[end,1])") - #println("curvature = $(getvalue(mdl.c))") - - # COST PRINTS: ******************************************************** - # println("coeff: $(getvalue(mdl.coeff))") - # println("z0: $(getvalue(mdl.z0))") - # println("Solution status: $sol_status") - # println("Objective value: $(getobjectivevalue(mdl.mdl))") - # println("Control Cost: $(getvalue(controlCost))") - # println("CostZ: $(getvalue(costZ))") - # println("DerivCost: $(getvalue(derivCost))") - # println("LaneCost: $(getvalue(laneCost))") - # println("costZTerm: $(getvalue(costZTerm))") - # println("constZTerm: $(getvalue(constZTerm))") - - # println("cost_ey: $(0.5*sum(sol_z[2,:].^2)*Q[2])") - # println("cost_ePsi: $(0.5*sum(sol_z[3,:].^2)*Q[3])") - # println("cost_V: $(0.5*sum((sol_z[4,:]-z_Ref[:,4]').^2)*Q[4])") - - #println("z:") - #println(getvalue(mdl.z_Ol)) - #println("u:") - #println(getvalue(mdl.u_Ol)) - #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1],sol_status,getvalue(mdl.u_Ol),getvalue(mdl.z_Ol),[getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)]) - mpcSol.a_x = sol_u[1,1] - mpcSol.d_f = sol_u[1,2] - mpcSol.u = sol_u - mpcSol.z = sol_z - mpcSol.solverStatus = sol_status - mpcSol.cost = zeros(6) - #mpcSol.cost = [getvalue(costZ),getvalue(costZTerm),getvalue(constZTerm),getvalue(derivCost),getvalue(controlCost),getvalue(laneCost)] - #mpcSol = MpcSol(sol_u[1,1],sol_u[2,1]) # Fast version without logging - #println(getvalue(costZTerm)) - #println(getvalue(mdl.z_Ol[1,N+1])) - #println(getvalue(constZTerm)) - #println("mdl.ParInt[1] = $(getvalue(mdl.ParInt[1]))") - #println("u = $(sol_u[:,1])") - - # if lapStatus.currentLap > 100 - # ss = collect(zCurr[1]:.01:zCurr[1]+0.3) - # #p = [ss.^4 ss.^3 ss.^2 ss.^1 ss.^0] *coeffTermCost[:,:,1] - # p = [ss.^4 ss.^3 ss.^2 ss.^1 ss.^0] *coeffTermConst[:,:,1,1] - # plot(ss,p,getvalue(mdl.z_Ol[1,N+1]),getvalue(costZTerm),"o") - # grid() - # readline() - # end - - # println(getvalue(mdl.z_Ol)) - # println("==============") - # println(getvalue(mdl.u_Ol)) - nothing -end From 804307f8916ab9af6b6935f4b7fbc5ee3b68cf5b Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 5 Oct 2016 18:19:04 -0700 Subject: [PATCH 041/183] Merged (dynamic) simulation node with submodule --- workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 9 +- workspace/src/barc/src/barc_simulator.jl | 2 + workspace/src/barc/src/barc_simulator_dyn.jl | 88 ++++--------------- .../src/barc/src/state_estimation_DynBkMdl.py | 2 + 5 files changed, 24 insertions(+), 79 deletions(-) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index e16cbfb5..6013837d 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -61,7 +61,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 6b1921fd..476df36a 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -12,11 +12,10 @@ using JuMP using Ipopt using JLD -include("helper/classes.jl") -include("helper/coeffConstraintCost.jl") -include("helper/solveMpcProblem.jl") -include("helper/computeCostLap.jl") -include("helper/functions.jl") +include("LMC_lib/classes.jl") +include("LMC_lib/coeffConstraintCost.jl") +include("LMC_lib/solveMpcProblem.jl") +include("LMC_lib/functions.jl") function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 472b6f2f..4e343da3 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -25,6 +25,8 @@ using geometry_msgs.msg using sensor_msgs.msg using JLD +include("LMPC_lib/simModel.jl") + u_current = zeros(2,1) t = 0 diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index c0d7a060..f36d0377 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -25,11 +25,13 @@ using geometry_msgs.msg using sensor_msgs.msg using JLD +include("LMPC_lib/classes.jl") +include("LMPC_lib/simModel.jl") + u_current = zeros(2,1) t = 0 - # This type contains measurement data (time, values and a counter) type Measurements{T} i::Int64 # measurement counter @@ -37,14 +39,6 @@ type Measurements{T} z::Array{T} # measurement values end -type ModelParams - l_A::Float64 - l_B::Float64 - m::Float64 - I_z::Float64 - v_steer::Float64 -end - # This function cleans the zeros from the type above once the simulation is finished function clean_up(m::Measurements) m.t = m.t[1:m.i-1] @@ -67,62 +61,6 @@ est_meas.t[1] = time() est_meas_dyn.t[1] = time() cmd_log.t[1] = time() -function pacejka(a) - B = 0.3#20 - C = 1.25 - mu = 0.234 - m = 1.98 - g = 9.81 - D = mu * m * g/2 - D = D*100 - - C_alpha_f = D*sin(C*atan(B*a)) - return C_alpha_f -end - -function simDynModel_exact(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) - dtn = dt/10 - t = 0:dtn:dt - z_final = z - ang = zeros(2) - for i=1:length(t)-1 - z_final, ang = simDynModel(z_final,u,dtn,modelParams) - end - return z_final, ang -end - -function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) - - zNext::Array{Float64} - L_f = modelParams.l_A - L_r = modelParams.l_B - m = modelParams.m - I_z = modelParams.I_z - v_steer = modelParams.v_steer # 0.5 rad / 0.2 seconds - - a_F = 0 - a_R = 0 - if abs(z[3]) > 0.1 - a_F = atan((z[4] + L_f*z[6])/z[3]) - z[7] - a_R = atan((z[4] - L_r*z[6])/z[3]) - end - - FyF = -pacejka(a_F) - FyR = -pacejka(a_R) - - zNext = z - # compute next state - zNext[1] = zNext[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) - zNext[2] = zNext[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) - zNext[3] = zNext[3] + dt * (u[1] + z[4]*z[6] - 0.63*z[3]^2*sign(z[3])) - zNext[4] = zNext[4] + dt * (2/m*(FyF*cos(z[7]) + FyR) - z[6]*z[3]) - zNext[5] = zNext[5] + dt * (z[6]) - zNext[6] = zNext[6] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) - zNext[7] = zNext[7] + dt * v_steer * sign(u[2]-z[7]) - - return zNext, [a_F a_R] -end - function ECU_callback(msg::ECU) global u_current u_current = [msg.motor, msg.servo] @@ -152,11 +90,6 @@ function main() pub_gps = Publisher("indoor_gps", Vector3, queue_size=10) pub_imu = Publisher("imu/data", Imu, queue_size=10) - - # read the axle distances from the launch file - l_A = get_param("L_a") # distance from CoG to front axel - l_B = get_param("L_b") # distance from CoG to rear axel - s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=10) @@ -183,15 +116,24 @@ function main() imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) - modelParams = ModelParams(0.125,0.125,1.98,0.24,0.5/0.2) # L_f, L_r, m, I_z, v_steer + modelParams = ModelParams() + # modelParams.l_A = copy(get_param("L_a")) # always throws segmentation faults *after* execution!!! ?? + # modelParams.l_B = copy(get_param("L_a")) + # modelParams.m = copy(get_param("m")) + # modelParams.I_z = copy(get_param("I_z")) + + modelParams.l_A = 0.125 + modelParams.l_B = 0.125 + modelParams.m = 1.98 + modelParams.I_z = 0.24 println("Publishing sensor information. Simulator running.") while ! is_shutdown() t = time() # update current state with a new row vector - z_current[i,:],slip_ang[i,:] = simDynModel_exact(z_current[i-1,:],u_current', dt, modelParams) - p#rintln("z_current:") + z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:],u_current', dt, modelParams) + #println("z_current:") #println(z_current[i,:]) #println(slip_ang[i,:]) diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 8dbc50de..f3ee6850 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -210,6 +210,8 @@ def state_estimation(): else: z_EKF[0] = float(x_meas) z_EKF[1] = float(y_meas) + z_EKF[6] = float(x_meas) # predicted values (these are published) + z_EKF[7] = float(y_meas) # wait rate.sleep() From 3f904f079bae60ff90da1c1b6418c7fa0dab9256 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 5 Oct 2016 18:25:43 -0700 Subject: [PATCH 042/183] Updated submodule reference --- workspace/src/barc/src/LMPC_lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/workspace/src/barc/src/LMPC_lib b/workspace/src/barc/src/LMPC_lib index edbf8b7f..685772c1 160000 --- a/workspace/src/barc/src/LMPC_lib +++ b/workspace/src/barc/src/LMPC_lib @@ -1 +1 @@ -Subproject commit edbf8b7ff8f04976f8bb311ba0886dd30235554b +Subproject commit 685772c144226a4741e00fb3d8b436b6920ef234 From 56ca47b15afb63df85201b6aab6da55748f1f032 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 6 Oct 2016 10:22:11 -0700 Subject: [PATCH 043/183] Adapted LMPC node to new submodule. LMPC node now uses dynamic estimation data. Adapted localization node so that it publishes dynamic estimation data. --- workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/msg/pos_info.msg | 3 + workspace/src/barc/src/LMPC_node.jl | 236 ++++++++++-------- .../src/barc/src/Localization_helpers.py | 16 +- workspace/src/barc/src/Localization_node.py | 21 +- workspace/src/barc/src/barc_simulator.jl | 2 +- .../src/barc/src/state_estimation_DynBkMdl.py | 4 +- workspace/src/barc/src/system_models.py | 3 + 8 files changed, 166 insertions(+), 121 deletions(-) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 6013837d..e16cbfb5 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -61,7 +61,7 @@ - + diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index ada342f5..7fbcb2a7 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -7,7 +7,10 @@ float64 v float64 s_start float64 x float64 y +float64 v_x +float64 v_y float64 psi +float64 psiDot float64[] coeffX float64[] coeffY float64[] coeffTheta diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 476df36a..5f2f61c3 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -1,7 +1,7 @@ #!/usr/bin/env julia using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Logging @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 rostypegen() @@ -12,15 +12,16 @@ using JuMP using Ipopt using JLD -include("LMC_lib/classes.jl") -include("LMC_lib/coeffConstraintCost.jl") -include("LMC_lib/solveMpcProblem.jl") -include("LMC_lib/functions.jl") +include("LMPC_lib/classes.jl") +include("LMPC_lib/MPC_models.jl") +include("LMPC_lib/coeffConstraintCost.jl") +include("LMPC_lib/solveMpcProblem.jl") +include("LMPC_lib/functions.jl") function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data # update mpc initial condition - z_est[:] = [msg.s,msg.ey,msg.epsi,msg.v] # use z_est as pointer + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer s_start_update[1] = msg.s_start coeffCurvature_update[:] = msg.coeffCurvature x_est[:] = [msg.x,msg.y,msg.psi,msg.v] @@ -29,18 +30,13 @@ function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature end function main() - println("now starting the node") + println("Starting LMPC node.") buffersize = 1000 # size of oldTraj buffers - # Create data to be saved - log_oldTraj = zeros(buffersize,4,2,20) # max. 10 laps - log_t = zeros(10000,1) - log_state = zeros(10000,4) - log_pred_z = zeros(10000,4) - log_cost = zeros(10000,6) - # Define and initialize variables + # --------------------------------------------------------------- + # General LMPC variables oldTraj = OldTrajectory() posInfo = PosInfo() mpcCoeff = MpcCoeff() @@ -49,33 +45,49 @@ function main() trackCoeff = TrackCoeff() # info about track (at current position, approximated) modelParams = ModelParams() mpcParams = MpcParams() + mpcParams_pF = MpcParams() # for 1st lap (path following) + + buffersize = 700 - z_est = zeros(4) - x_est = zeros(4) - coeffX = zeros(9) - coeffY = zeros(9) - s_start_update = [0.0] - cmd = ECU(0.0,0.0) + z_Init = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] # xDot needs to be > 0 + z_Init_pF = zeros(4) - InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff,z_Init) + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff,z_Init_pF) - log_coeff_Cost = zeros(mpcCoeff.order+1,2,10000) - log_coeff_Const = zeros(mpcCoeff.order+1,2,3,10000) - log_sol_z = zeros(mpcParams.N+1,4,10000) - log_sol_u = zeros(mpcParams.N,2,10000) + z_ID = zeros(50,6) + u_ID = zeros(50,2) + # ROS-specific variables + z_est = zeros(6) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) + x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) + coeffX = zeros(9) # buffer for coeffX (only logging) + coeffY = zeros(9) # buffer for coeffY (only logging) + s_start_update = [0.0] # buffer for s_start + cmd = ECU(0.0,0.0) # command type coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) + + # Logging variables + log_coeff_Cost = zeros(mpcCoeff.order+1,2,10000) + log_coeff_Const = zeros(mpcCoeff.order+1,2,5,10000) + log_sol_z = zeros(mpcParams.N+1,6,10000) + log_sol_u = zeros(mpcParams.N,2,10000) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) log_s_start = zeros(10000) log_state_x = zeros(10000,4) log_coeffX = zeros(10000,9) log_coeffY = zeros(10000,9) + log_t = zeros(10000,1) + log_state = zeros(10000,6) + log_cost = zeros(10000,6) + log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps + # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) pub = Publisher("ecu", ECU, queue_size=10) - pub2 = Publisher("logging", Logging, queue_size=10) - # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: + # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=10) println("Finished initialization.") @@ -84,91 +96,69 @@ function main() s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] # buffer in current lap - zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) + zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information + u_final = zeros(2) # contains last input of one lap - zCurr_export = zeros(buffersize,4) - uCurr_export = zeros(buffersize,2) - + # Specific initializations: + lapStatus.currentLap = 1 + lapStatus.currentIt = 1 + posInfo.s_target = 36.84 + k = 0 # overall counter for logging - # DEFINE MODEL *************************************************************************** - # **************************************************************************************** - println("Building model...") - - z_Init = zeros(4) - - mdl = MpcModel() - InitializeModel(mdl,mpcParams,modelParams,trackCoeff,z_Init) - - # Initial solve: - println("Initial solve...") - solve(mdl.mdl) - solve(mdl.mdl) - println("Finished.") - - pred_z = zeros(4) - - k = 0 - - # Precompile functions - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams) - extendOldTraj(oldTraj,posInfo,zCurr) - lapStatus.currentIt = 100 - saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) - lapStatus.currentIt = 0 - mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) # ... and set them back to zeros - mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,3) - println("Finished precompiling.") - # Start node while ! is_shutdown() - if z_est[1] > 0 # check if data has been received (s > 0) + if z_est[6] > 0 # check if data has been received (s > 0) + println("Received data. Starting one iteration.") - # publish command from last calculation + # ============================= PUBLISH COMMANDS ============================= + # this is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received + # the state is predicted by 0.1s + println("Publishing.") cmd.motor = mpcSol.a_x cmd.servo = mpcSol.d_f publish(pub, cmd) # ============================= Initialize iteration parameters ============================= - lapStatus.currentIt += 1 # count iteration - - i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr[i,:] = z_est # update state information: s, e_y, e_psi, v (actually predicted by Kalman filter!) - posInfo.s = z_est[1] # update position info - posInfo.s_start = s_start_update[1] - trackCoeff.coeffCurvature = coeffCurvature_update + println("Initializing iteration parameters.") + i = lapStatus.currentIt # current iteration number, just to make notation shorter + zCurr[i,:] = copy(z_est) # update state information (actually predicted by Kalman filter!) + posInfo.s = zCurr[i,6] # update position info + posInfo.s_start = copy(s_start_update[1]) # use a copy so s_start is not updated during one iteration + trackCoeff.coeffCurvature = copy(coeffCurvature_update) # ======================================= Lap trigger ======================================= - # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... - # ... then select and save data println("Saving data") tic() + # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] - zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state - uCurr[1,:] = uCurr[i+1,:] # ... and input - i = 1 - lapStatus.currentLap += 1 # start next lap + zCurr[1,:] = zCurr[i,:] # copy current state + u_final  = uCurr[i,:] # ... and input + i = 1 lapStatus.currentIt = 1 # reset current iteration + lapStatus.currentLap += 1 # start next lap switchLap = false tt = toq() - println("Saved data, t = $tt") - println("======================================== NEXT LAP ========================================") - println("cost: $(oldTraj.oldCost)") - println("oldTraj.oldTraj[:,1,1]:") - println(oldTraj.oldTraj[:,1,1]) - println("oldTraj.oldTraj[:,1,2]:") - println(oldTraj.oldTraj[:,1,2]) + # println("Saved data, t = $tt") + # println("======================================== NEXT LAP ========================================") + # println("cost: $(oldTraj.oldCost)") + # println("oldTraj.oldTraj[:,1,1]:") + # println(oldTraj.oldTraj[:,1,1]) + # println("oldTraj.oldTraj[:,1,2]:") + # println(oldTraj.oldTraj[:,1,2]) elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger switchLap = true end + println("Starting one iteration.") - # if we are at least in the 2nd lap, concatenate the beginning to the end of the previous track - if lapStatus.currentLap > 1 && lapStatus.currentIt == 80 - extendOldTraj(oldTraj,posInfo,zCurr) - end + # For System ID: Update last 50 measurements + z_ID = circshift(z_ID,-1) + u_ID = circshift(u_ID,-1) + z_ID[end,:] = zCurr[i,:] + u_ID[end,:] = uCurr[i,:] # ======================================= Calculate input ======================================= println("======================================== NEW ITERATION # $i ========================================") @@ -180,50 +170,68 @@ function main() println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") # Find coefficients for cost and constraints - if lapStatus.currentLap > 1 tic() - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) tt = toq() println("Finished coefficients, t = $tt s") end + # Find last inputs u_i-1 + if i>1 # last u is needed for smooth MPC solutions (for derivative of u) + last_u = uCurr[i-1,:] + else + last_u = u_final + end + # Solve the MPC problem tic() - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,pred_z,uCurr[i,:]') + if lapStatus.currentLap <= 2 + z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],zCurr[i,1]] # use kinematic model and its states + solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,last_u') + else # otherwise: use system-ID-model + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') + end + + #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') tt = toq() # Write in current input information - uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i+1,:]), t = $tt s") + uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) - println("\n") - #println("Starting logging") - + + # append new states and inputs to old trajectory + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target + oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = uCurr[i,:] + if lapStatus.currentLap==3 # if its the third lap, append to both old trajectories! (since both are the same) + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = zCurr[i,:] + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,2] += posInfo.s_target + oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] + end + # Logging + # --------------------------- k = k + 1 # counter log_state[k,:] = z_est log_t[k] = time() - log_sol_z[:,:,k] = mpcSol.z log_sol_u[:,:,k] = mpcSol.u log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst log_cost[k,:] = mpcSol.cost log_curv[k,:] = trackCoeff.coeffCurvature log_s_start[k] = posInfo.s_start - log_pred_z[k,:] = pred_z log_state_x[k,:] = x_est - #log_coeffX[k,:] = coeffX - #log_coeffY[k,:] = coeffY - #println("Finished logging") - - # publish command from last calculation - #cmd.motor = mpcSol.a_x - #cmd.servo = mpcSol.d_f - #publish(pub, cmd) - #println("z_pred = $(pred_z')") + if lapStatus.currentLap <= 2 + log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) + else + log_sol_z[:,1:6,k] = mpcSol.z + end + + # Count one up: + lapStatus.currentIt += 1 else println("No estimation data received!") end @@ -234,7 +242,7 @@ function main() log_path = "$(homedir())/simulations/output_LMPC.jld" save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, - "s_start",log_s_start[1:k],"pred_z",log_pred_z[1:k,:],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:]) + "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:]) println("Exiting LMPC node. Saved data.") end @@ -242,3 +250,17 @@ end if ! isinteractive() main() end + +# Sequence within one iteration: +# 1. Publish commands from last iteration (because the car is in real *now* where we thought it was before (predicted state)) +# 2. Receive new state information +# 3. Check if we've crossed the finish line and if we have, switch lap number and save old trajectories +# 4. (If in 3rd lap): Calculate coefficients +# 5. Calculate MPC commands (depending on lap) which are going to be published in the next iteration +# 6. (Do some logging) + + +# Definitions of variables: +# zCurr contains all state information from the beginning of the lap (first s >= 0) to the current state i +# uCurr -> same as zCurr for inputs +# generally: zCurr[i+1] = f(zCurr[i],uCurr[i]) \ No newline at end of file diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 15024c9b..d7ce9ab2 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -37,10 +37,10 @@ class Localization: OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? - coeffX = zeros(11) - coeffY = zeros(11) - coeffTheta = 0 - coeffCurvature = 0 + coeffX = zeros(9) + coeffY = zeros(9) + coeffTheta = zeros(9) + coeffCurvature = zeros(9) s = 0 # distance from s_start to current closest node (idx_min) s_start = 0 # distance along path from first node to start node (which is N_nodes_poly_back behind current closest node) @@ -49,6 +49,9 @@ class Localization: v = 0 # current velocity (not necessary for these calculations but for MPC) x = 0 y = 0 + v_x = 0 + v_y = 0 + psiDot = 0 def create_circle(self,rad=1.0,n=100,c=array([0,0])): # note: need to make sure that all points are equidistant! ang = linspace(0,2*pi-2*pi/n,n) @@ -215,12 +218,15 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e #print(self.nodes) - def set_pos(self,x,y,psi,v): + def set_pos(self,x,y,psi,v,v_x,v_y,psiDot): self.pos = array([x,y]) self.psi = psi self.v = v self.x = x self.y = y + self.v_x = v_x + self.v_y = v_y + self.psiDot = psiDot def find_s(self): dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of current position to all nodes diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index d6db90dd..c67de76b 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -10,17 +10,27 @@ import os from numpy import * from Localization_helpers import Localization -from barc.msg import Z_KinBkMdl, pos_info +from barc.msg import Z_KinBkMdl, pos_info, Z_DynBkMdl import matplotlib.pyplot as plt l = 0 epsi_prev = 0 # State estimate callback function -def state_estimate_callback(data): +# def state_estimate_callback(data): +# global l, epsi_prev +# # Set current position and orientation +# l.set_pos(data.x,data.y,data.psi,data.v) +# # Update position and trajectory information +# l.find_s() +# # unwrap epsi +# # l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] +# epsi_prev = l.epsi + +def dyn_state_estimate_callback(data): global l, epsi_prev # Set current position and orientation - l.set_pos(data.x,data.y,data.psi,data.v) + l.set_pos(data.x,data.y,data.psi,data.v_x,data.v_x,data.v_y,data.psi_dot) # v = v_x # Update position and trajectory information l.find_s() # unwrap epsi @@ -35,7 +45,8 @@ def localization_node(): rospy.init_node('localization', anonymous=True) # topic subscriptions / publications - rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback) + #rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback) + rospy.Subscriber('state_estimate_dynamic', Z_DynBkMdl, dyn_state_estimate_callback) state_pub = rospy.Publisher('pos_info', pos_info, queue_size = 10) # create localization class and trajectory @@ -59,7 +70,7 @@ def localization_node(): while not rospy.is_shutdown(): # publish information - state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.psi,l.coeffX,l.coeffY,l.coeffTheta,l.coeffCurvature) ) + state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.v_x,l.v_y,l.psi,l.psiDot,l.coeffX.tolist(),l.coeffY.tolist(),l.coeffTheta.tolist(),l.coeffCurvature.tolist()) ) # wait rate.sleep() diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 4e343da3..71045209 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -14,7 +14,7 @@ =# using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl +@rosimport barc.msg: ECU, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index f3ee6850..5480b0d9 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -184,8 +184,8 @@ def state_estimation(): (x,y,v_x,v_y,psi,psi_dot,x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) = z_EKF # note, r = EKF estimate yaw rate # publish information - #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) - state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) + state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) + #state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) # apply EKF #if v_x_enc > v_x_min: diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 7115b9a3..a4279902 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -276,6 +276,9 @@ def f_DynBkMdl(z,u,vhMdl,trMdl,dt): FyF = -pacejka(a_F) FyR = -pacejka(a_R) + if abs(a_F) > 30/180*3.1416 or abs(a_R) > 30/180*3.1416: + print("WARNING: Large slip angles in estimation") + # compute next state x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) From ffe385f80a8c5c0e384a3b49d65ff8badde0b6b1 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 6 Oct 2016 10:23:07 -0700 Subject: [PATCH 044/183] Updated submodule. --- workspace/src/barc/src/LMPC_lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/workspace/src/barc/src/LMPC_lib b/workspace/src/barc/src/LMPC_lib index 685772c1..3ecf9b11 160000 --- a/workspace/src/barc/src/LMPC_lib +++ b/workspace/src/barc/src/LMPC_lib @@ -1 +1 @@ -Subproject commit 685772c144226a4741e00fb3d8b436b6920ef234 +Subproject commit 3ecf9b112d3c540b180a47a13475cd84eb6902a4 From 09e9959dd5726d9c87f74f43a9a84198b8d46fbf Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 6 Oct 2016 18:00:49 -0700 Subject: [PATCH 045/183] Adapted evaluation and added psi measurement to Kalman filter. LMPC working but still issues with yDot estimation. --- eval_sim/eval_sim.jl | 107 ++++++++---------- workspace/src/barc/launch/barc_sim.launch | 6 +- workspace/src/barc/msg/state_info.msg | 6 + workspace/src/barc/src/LMPC_node.jl | 50 ++++---- .../src/barc/src/Localization_helpers.py | 44 +++---- workspace/src/barc/src/Localization_node.py | 23 ++-- workspace/src/barc/src/barc_simulator_dyn.jl | 4 +- .../src/barc/src/state_estimation_DynBkMdl.py | 11 +- workspace/src/barc/src/system_models.py | 92 +++++++++++---- 9 files changed, 194 insertions(+), 149 deletions(-) create mode 100644 workspace/src/barc/msg/state_info.msg diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index d2539a59..8c6967c1 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -74,19 +74,32 @@ function eval_sim() legend(["x","y","v_x","v_y","psi","psi_dot","x","y","psi","v","x","y","v_x","v_y","psi","psi_dot","d_f"]) figure() - plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,3],est_dyn.t,est_dyn.z[:,5:6],"-*") - legend(["psi_imu","psi_dot_imu","psi_true","psi_est","psi_dot_est"]) + title("Comparison of psi") + plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],est_dyn.t,est_dyn.z[:,5:6],"-*") + legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) grid() figure() - plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*",z.z[:,1],z.z[:,2],"-") - grid(1) - legend(["est","est_dyn","true"]) + title("Comparison of v") + plot(z.t,z.z[:,3:4],est_dyn.t,est_dyn.z[:,3:4],"-*") + legend(["real_xDot","real_yDot","est_xDot","est_yDot"]) + grid() figure() - plot(z.t-t0,z.z[:,5],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) - grid(1) - legend(["Real psi","psi meas","estimate"]) + title("Comparison of x,y") + plot(z.t,z.z[:,1:2],est_dyn.t,est_dyn.z[:,1:2],"-*") + legend(["real_x","real_y","est_x","est_y"]) + grid() + + #figure() + #plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*",z.z[:,1],z.z[:,2],"-") + #grid(1) + #legend(["est","est_dyn","true"]) + + #figure() + #plot(z.t-t0,z.z[:,5],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) + #grid(1) + #legend(["Real psi","psi meas","estimate"]) figure() plot(cmd_log.t-t0,cmd_log.z) @@ -105,7 +118,7 @@ function eval_LMPC() sol_u = d_lmpc["sol_u"] cost = d_lmpc["cost"] curv = d_lmpc["curv"] - f_z = d_lmpc["pred_z"] + x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] @@ -136,17 +149,6 @@ function eval_LMPC() ylabel("Curvature") grid() - figure() - plot(z.t-t0,z.z[:,[1,2,5]],"-x",est.t-t0,est.z[:,1:3],"-*") - grid(1) - legend(["x","y","psi","x_est","y_est","psi_est"]) - - figure() - plot(z.t-t0,z.z[:,5],t-t0,x_est[:,3]) - title("Comparison heading angle") - legend(["psi_true","psi_est"]) - grid() - track = create_track(0.3) figure() hold(1) @@ -179,24 +181,15 @@ function eval_LMPC() # plot(x,y) # end - rg = 100:500 - figure() - plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") - title("Comparison states and prediction") - legend(["ey","epsi","v"]) - grid(1) - for i=100:5:500 - plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") - end - figure() - ax1=subplot(211) - plot(t-t0,state,"-o",t-t0,f_z,"-*") - legend(["s","ey","epsi","v","s_pred","ey_pred","epsi_pred","v_pred"]) - grid(1) - subplot(212,sharex = ax1) - plot(t-t0,reshape(sol_u[1,:,:],2,size(sol_u,3))') - grid(1) - + # rg = 100:500 + # figure() + # plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") + # title("Comparison states and prediction") + # legend(["ey","epsi","v"]) + # grid(1) + # for i=100:5:500 + # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") + # end figure() plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") legend(["e_y","e_psi","v"]) @@ -363,27 +356,27 @@ function create_track(w) theta = [0.0] # SOPHISTICATED TRACK - add_curve(theta,30,0.0) - add_curve(theta,60,-2*pi/3) - add_curve(theta,90,pi) - add_curve(theta,80,-5*pi/6) - add_curve(theta,10,0.0) - add_curve(theta,50,-pi/2) - add_curve(theta,50,0.0) - add_curve(theta,40,-pi/4) - add_curve(theta,30,pi/4) - add_curve(theta,20,0.0) - add_curve(theta,50,-pi/2) - add_curve(theta,25,0.0) - add_curve(theta,50,-pi/2) - add_curve(theta,28,0.0) + # add_curve(theta,30,0.0) + # add_curve(theta,60,-2*pi/3) + # add_curve(theta,90,pi) + # add_curve(theta,80,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,50,0.0) + # add_curve(theta,40,-pi/4) + # add_curve(theta,30,pi/4) + # add_curve(theta,20,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,25,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,28,0.0) # SIMPLE track - # add_curve(theta,50,0) - # add_curve(theta,100,-pi) - # add_curve(theta,100,0) - # add_curve(theta,100,-pi) - # add_curve(theta,49,0) + add_curve(theta,50,0) + add_curve(theta,100,-pi) + add_curve(theta,100,0) + add_curve(theta,100,-pi) + add_curve(theta,49,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index e16cbfb5..f2f4d618 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -46,11 +46,11 @@ - - + + - + diff --git a/workspace/src/barc/msg/state_info.msg b/workspace/src/barc/msg/state_info.msg new file mode 100644 index 00000000..28bdeb8d --- /dev/null +++ b/workspace/src/barc/msg/state_info.msg @@ -0,0 +1,6 @@ +float64 x +float64 y +float64 xDot +float64 yDot +float64 psi +float64 psiDot \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 5f2f61c3..231c1e8e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -47,8 +47,6 @@ function main() mpcParams = MpcParams() mpcParams_pF = MpcParams() # for 1st lap (path following) - buffersize = 700 - z_Init = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] # xDot needs to be > 0 z_Init_pF = zeros(4) @@ -103,24 +101,32 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 36.84 + posInfo.s_target = 24.0 k = 0 # overall counter for logging + # Precompile coeffConstraintCost: + oldTraj.oldTraj[1:buffersize,6,1] = 1:buffersize + oldTraj.oldTraj[1:buffersize,6,2] = 1:buffersize + posInfo.s = 400 + posInfo.s_start = 0 + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) + oldTraj.oldTraj[1:buffersize,6,1] = zeros(buffersize,1) + oldTraj.oldTraj[1:buffersize,6,2] = zeros(buffersize,1) + posInfo.s = 0 + posInfo.s_start = 0 + # Start node while ! is_shutdown() if z_est[6] > 0 # check if data has been received (s > 0) - println("Received data. Starting one iteration.") # ============================= PUBLISH COMMANDS ============================= # this is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received # the state is predicted by 0.1s - println("Publishing.") cmd.motor = mpcSol.a_x cmd.servo = mpcSol.d_f publish(pub, cmd) # ============================= Initialize iteration parameters ============================= - println("Initializing iteration parameters.") i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information (actually predicted by Kalman filter!) posInfo.s = zCurr[i,6] # update position info @@ -129,30 +135,22 @@ function main() # ======================================= Lap trigger ======================================= if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... - println("Saving data") - tic() + println("Finishing one lap at iteration $i") + println("current state: $(zCurr[i,:])") + println("previous state: $(zCurr[i-1,:])") # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj - saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,modelParams.dt) + saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,posInfo,buffersize) + println("oldTraj: $(oldTraj.oldTraj[:,:,1])") log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] zCurr[1,:] = zCurr[i,:] # copy current state - u_final  = uCurr[i,:] # ... and input + u_final  = uCurr[i-1,:] # ... and input i = 1 lapStatus.currentIt = 1 # reset current iteration lapStatus.currentLap += 1 # start next lap switchLap = false - - tt = toq() - # println("Saved data, t = $tt") - # println("======================================== NEXT LAP ========================================") - # println("cost: $(oldTraj.oldCost)") - # println("oldTraj.oldTraj[:,1,1]:") - # println(oldTraj.oldTraj[:,1,1]) - # println("oldTraj.oldTraj[:,1,2]:") - # println(oldTraj.oldTraj[:,1,2]) elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger switchLap = true end - println("Starting one iteration.") # For System ID: Update last 50 measurements z_ID = circshift(z_ID,-1) @@ -164,13 +162,12 @@ function main() println("======================================== NEW ITERATION # $i ========================================") println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") println("State Nr. $i = $z_est") - println("Coeff Curvature = $(trackCoeff.coeffCurvature)") println("s = $(posInfo.s)") println("s_start = $(posInfo.s_start)") println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") # Find coefficients for cost and constraints - if lapStatus.currentLap > 1 + if lapStatus.currentLap > 2 tic() coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) tt = toq() @@ -192,15 +189,12 @@ function main() else # otherwise: use system-ID-model solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') end - - #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - tt = toq() - # Write in current input information - uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") - zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) + # Write current input information + uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] + zCurr[i,6] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) # append new states and inputs to old trajectory oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index d7ce9ab2..b63cc6e8 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -28,7 +28,7 @@ class Localization: c = 0 # center of circle (in case of circular trajectory) pos = 0 # current position psi = 0 # current orientation - nodes = 0 # all nodes are saved in a matrix + nodes = array([0]) # all nodes are saved in a matrix N_nodes_poly_back = 20 # number of nodes behind current position N_nodes_poly_front = 40 # number of nodes in front ds = 0 # distance between nodes @@ -109,28 +109,28 @@ def create_track(self): theta = array([0]) # Sophisticated racetrack: length = 25.62m - theta = add_curve(theta,30,0) - theta = add_curve(theta,60,-2*pi/3) - #theta = add_curve(theta,30,0) - theta = add_curve(theta,90,pi) - theta = add_curve(theta,80,-5*pi/6) - theta = add_curve(theta,10,0) - theta = add_curve(theta,50,-pi/2) - theta = add_curve(theta,50,0) - theta = add_curve(theta,40,-pi/4) - theta = add_curve(theta,30,pi/4) - theta = add_curve(theta,20,0) - theta = add_curve(theta,50,-pi/2) - theta = add_curve(theta,25,0) - theta = add_curve(theta,50,-pi/2) - theta = add_curve(theta,28,0) + # theta = add_curve(theta,30,0) + # theta = add_curve(theta,60,-2*pi/3) + # #theta = add_curve(theta,30,0) + # theta = add_curve(theta,90,pi) + # theta = add_curve(theta,80,-5*pi/6) + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,50,-pi/2) + # theta = add_curve(theta,50,0) + # theta = add_curve(theta,40,-pi/4) + # theta = add_curve(theta,30,pi/4) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,50,-pi/2) + # theta = add_curve(theta,25,0) + # theta = add_curve(theta,50,-pi/2) + # theta = add_curve(theta,28,0) # SIMPLE RACETRACK (smooth curves): length = 24.0m - # theta = add_curve(theta,50,0) - # theta = add_curve(theta,100,-pi) - # theta = add_curve(theta,100,0) - # theta = add_curve(theta,100,-pi) - # theta = add_curve(theta,49,0) + theta = add_curve(theta,50,0) + theta = add_curve(theta,100,-pi) + theta = add_curve(theta,100,0) + theta = add_curve(theta,100,-pi) + theta = add_curve(theta,49,0) # SIMPLER RACETRACK (half circles as curves): @@ -147,7 +147,7 @@ def create_track(self): def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines s = size(x) - da = 2*pi/360*5 + da = 2.0*pi/360*5 x = hstack((x,L/2.0+b/2*cos(arange(pi/2,-pi/2,-da)))) x = hstack((x,linspace(L/2.0,-L/2.0,5))) x = hstack((x,-L/2.0+b/2*cos(arange(pi/2,1.5*pi+da,da)))) diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index c67de76b..b9e30b68 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -15,6 +15,7 @@ l = 0 epsi_prev = 0 +running = False # State estimate callback function # def state_estimate_callback(data): @@ -28,19 +29,20 @@ # epsi_prev = l.epsi def dyn_state_estimate_callback(data): - global l, epsi_prev - # Set current position and orientation - l.set_pos(data.x,data.y,data.psi,data.v_x,data.v_x,data.v_y,data.psi_dot) # v = v_x - # Update position and trajectory information - l.find_s() - # unwrap epsi - # l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] - epsi_prev = l.epsi + global l, epsi_prev, running + if running: + # Set current position and orientation + l.set_pos(data.x,data.y,data.psi,data.v_x,data.v_x,data.v_y,data.psi_dot) # v = v_x + # Update position and trajectory information + l.find_s() + # unwrap epsi + # l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] + epsi_prev = l.epsi # localization node def localization_node(): global l # localization class - + global running # initialize node rospy.init_node('localization', anonymous=True) @@ -52,7 +54,7 @@ def localization_node(): # create localization class and trajectory l = Localization() # l.create_circle(1,100,array([3.2,0.5])) - #l.create_racetrack(3.0,3.0,0.2,array([0.0,-1.5]),0) + #l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) l.create_track() #l.create_track2() # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) @@ -68,6 +70,7 @@ def localization_node(): t0 = time.time() while not rospy.is_shutdown(): + running = True # publish information state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.v_x,l.v_y,l.psi,l.psiDot,l.coeffX.tolist(),l.coeffY.tolist(),l.coeffTheta.tolist(),l.coeffCurvature.tolist()) ) diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index f36d0377..25689100 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -47,7 +47,7 @@ end buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) est_meas_dyn = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,6)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) @@ -161,7 +161,7 @@ function main() if i%2 == 0 imu_meas.i += 1 imu_meas.t[imu_meas.i] = t - imu_meas.z[imu_meas.i] = yaw + imu_meas.z[imu_meas.i,:] = [yaw z_current[i,6]] publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 5480b0d9..c91af73d 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -172,7 +172,7 @@ def state_estimation(): elif est_mode==3: # use gps only R = (gps_std**2)*eye(2) elif est_mode==4: # use gps and angular velocity - R = diag([gps_std,gps_std,ang_v_std,v_std])**2 + R = diag([gps_std,gps_std,ang_v_std,v_std,psi_std])**2 else: rospy.logerr("No estimation mode selected.") @@ -184,16 +184,15 @@ def state_estimation(): (x,y,v_x,v_y,psi,psi_dot,x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) = z_EKF # note, r = EKF estimate yaw rate # publish information - state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) - #state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) + #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) + state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) # apply EKF #if v_x_enc > v_x_min: if FxR > 0 or running: running = True # get measurement - y = array([x_meas,y_meas,w_z,v_x_enc]) - print "v_x_enc = %f"%v_x_enc + y = array([x_meas,y_meas,w_z,v_x_enc,yaw]) # define input u = array([ d_f, FxR ]) @@ -203,7 +202,7 @@ def state_estimation(): args = (u, vhMdl, TrMdl, dt) # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_DynBkMdl, z_EKF, P, h_DynBkMdl, y, Q, R, args ) + (z_EKF,P) = ekf(f_DynBkMdl_exact, z_EKF, P, h_DynBkMdl, y, Q, R, args ) #print "New Iteration ------" #print P diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index a4279902..3e2d7c4c 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -226,7 +226,7 @@ def pacejka(a): m = 1.98 g = 9.81 D = mu * m * g/2 - D = D*10 + D = D*50 C_alpha_f = D*sin(C*arctan(B*a)) return C_alpha_f @@ -240,23 +240,23 @@ def f_DynBkMdl_exact(z,u,vhMdl,trMdl,dt): return zNext def f_DynBkMdl(z,u,vhMdl,trMdl,dt): - x_I = z[0] - y_I = z[1] - v_x = z[2] - v_y = z[3] - psi = z[4] - psi_dot = z[5] - x_I_pred = z[6] - y_I_pred = z[7] - v_x_pred = z[8] - v_y_pred = z[9] - psi_pred = z[10] - psi_dot_pred = z[11] - - d_f = u[0] - a = u[1] - - dt_pred = 0.15 + x_I = z[0] + y_I = z[1] + v_x = z[2] + v_y = z[3] + psi = z[4] + psi_dot = z[5] + x_I_pred = z[6] + y_I_pred = z[7] + v_x_pred = z[8] + v_y_pred = z[9] + psi_pred = z[10] + psi_dot_pred = z[11] + + d_f = u[0] + a = u[1] + + dt_pred = 0.15 # extract parameters (L_f,L_r,m,I_z) = vhMdl @@ -276,8 +276,13 @@ def f_DynBkMdl(z,u,vhMdl,trMdl,dt): FyF = -pacejka(a_F) FyR = -pacejka(a_R) - if abs(a_F) > 30/180*3.1416 or abs(a_R) > 30/180*3.1416: - print("WARNING: Large slip angles in estimation") + #a_F = 0 # experimental: set all forces to zero + #a_R = 0 + #FyF = 0 + #FyR = 0 + + if abs(a_F) > 30.0/180.0*3.1416 or abs(a_R) > 30.0/180.0*3.1416: + print("WARNING: Large slip angles in estimation: a_F = %f, a_R = %f"%(a_F,a_R)) # compute next state x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) @@ -296,6 +301,50 @@ def f_DynBkMdl(z,u,vhMdl,trMdl,dt): return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next,x_I_next_pred,y_I_next_pred,v_x_next_pred,v_y_next_pred,psi_next_pred,psi_dot_next_pred]) +def f_DynBkMdl_Kin(z,u,vhMdl,trMdl,dt): + x_I = z[0] + y_I = z[1] + v_x = z[2] + v_y = z[3] + psi = z[4] + psi_dot = z[5] + x_I_pred = z[6] + y_I_pred = z[7] + v_x_pred = z[8] + v_y_pred = z[9] + psi_pred = z[10] + psi_dot_pred = z[11] + + d_f = u[0] + a = u[1] + + dt_pred = 0.15 + + # extract parameters + (L_f,L_r,m,I_z) = vhMdl + (trMdlFront, trMdlRear) = trMdl + (B,C,mu) = trMdlFront + g = 9.81 + Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) + + # compute next state + x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) + y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) + v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) + v_y_next = v_y + dt * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) + psi_next = psi + dt * (psi_dot) + psi_dot_next = psi_dot + dt * (2/I_z*(L_f*FyF - L_r*FyR)) + + x_I_next_pred = x_I + dt_pred * (cos(psi)*v_x - sin(psi)*v_y) + y_I_next_pred = y_I + dt_pred * (sin(psi)*v_x + cos(psi)*v_y) + v_x_next_pred = v_x + dt_pred * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) + v_y_next_pred = v_y + dt_pred * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) + psi_next_pred = psi + dt_pred * (psi_dot) + psi_dot_next_pred = psi_dot + dt_pred * (2/I_z*(L_f*FyF - L_r*FyR)) + + return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next,x_I_next_pred,y_I_next_pred,v_x_next_pred,v_y_next_pred,psi_next_pred,psi_dot_next_pred]) + + def f_KinBkMdl_predictive(z,u,vhMdl, dt): """ process model @@ -342,7 +391,8 @@ def h_DynBkMdl(x): C = array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]]) + [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]]) return dot(C, x) def h_KinBkMdl(x): From f661744f03fbacd2e73c38e2f539174883282f4c Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 7 Oct 2016 10:53:15 -0700 Subject: [PATCH 046/183] Small changes in simulation model --- eval_sim/eval_sim.jl | 3 +++ workspace/src/barc/src/LMPC_lib | 2 +- workspace/src/barc/src/LMPC_node.jl | 4 +++- workspace/src/barc/src/state_estimation_DynBkMdl.py | 4 ++-- workspace/src/barc/src/system_models.py | 8 ++++---- 5 files changed, 13 insertions(+), 8 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 8c6967c1..537a9468 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -2,6 +2,8 @@ using JLD using PyPlot using HDF5, JLD, ProfileView +include("../workspace/src/barc/src/LMPC_lib/classes.jl") + type Measurements{T} i::Int64 # measurement counter t::Array{Float64} # time data @@ -118,6 +120,7 @@ function eval_LMPC() sol_u = d_lmpc["sol_u"] cost = d_lmpc["cost"] curv = d_lmpc["curv"] + mpcCoeff = d_lmpc["mpcCoeff"] x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] diff --git a/workspace/src/barc/src/LMPC_lib b/workspace/src/barc/src/LMPC_lib index 3ecf9b11..a4a5f043 160000 --- a/workspace/src/barc/src/LMPC_lib +++ b/workspace/src/barc/src/LMPC_lib @@ -1 +1 @@ -Subproject commit 3ecf9b112d3c540b180a47a13475cd84eb6902a4 +Subproject commit a4a5f043a40e30e68bc82ea637b0116fca8679a5 diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 231c1e8e..247620bd 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -79,6 +79,7 @@ function main() log_t = zeros(10000,1) log_state = zeros(10000,6) log_cost = zeros(10000,6) + log_mpcCoeff = Array(MpcCoeff,10000) log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps # Initialize ROS node and topics @@ -218,6 +219,7 @@ function main() log_curv[k,:] = trackCoeff.coeffCurvature log_s_start[k] = posInfo.s_start log_state_x[k,:] = x_est + log_mpcCoeff[k] = mpcCoeff if lapStatus.currentLap <= 2 log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) else @@ -236,7 +238,7 @@ function main() log_path = "$(homedir())/simulations/output_LMPC.jld" save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, - "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:]) + "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"mpcCoeff",log_mpcCoeff) println("Exiting LMPC node. Saved data.") end diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index c91af73d..7c4d9928 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -184,8 +184,8 @@ def state_estimation(): (x,y,v_x,v_y,psi,psi_dot,x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) = z_EKF # note, r = EKF estimate yaw rate # publish information - #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) - state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) + state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) + #state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) # apply EKF #if v_x_enc > v_x_min: diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 3e2d7c4c..745d218a 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -220,13 +220,13 @@ def f_KinBkMdl(z,u,vhMdl, dt): return array([x_next, y_next, psi_next, v_next]) def pacejka(a): - B = 0.3#20 - C = 1.25 - mu = 0.234 + B = 1.0 + C = 1.0 + mu = 0.8 m = 1.98 g = 9.81 D = mu * m * g/2 - D = D*50 + D = D*10 C_alpha_f = D*sin(C*arctan(B*a)) return C_alpha_f From 4e2157ccc595c15854eec0396d0d94462657fd3e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 10 Oct 2016 09:21:17 -0700 Subject: [PATCH 047/183] Small changes in evaluation and logging, changed queue-sizes to 1, added noise to simulation. --- eval_sim/eval_sim.jl | 62 +++++++++----- workspace/src/barc/launch/barc_sim.launch | 20 +---- workspace/src/barc/src/LMPC_node.jl | 33 +++++--- .../src/barc/src/Localization_helpers.py | 19 +++-- workspace/src/barc/src/Localization_node.py | 2 +- workspace/src/barc/src/barc_simulator.jl | 19 ++--- workspace/src/barc/src/barc_simulator_dyn.jl | 34 ++++---- workspace/src/barc/src/observers.py | 3 +- .../src/barc/src/state_estimation_DynBkMdl.py | 84 +++++++++---------- workspace/src/barc/src/system_models.py | 13 +-- 10 files changed, 152 insertions(+), 137 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 537a9468..b3c70e23 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -36,7 +36,6 @@ end function eval_sim() d = load(log_path) - est = d["estimate"] est_dyn = d["estimate_dyn"] imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] @@ -44,7 +43,7 @@ function eval_sim() cmd_log = d["cmd_log"] slip_a = d["slip_a"] - t0 = est.t[1] + t0 = est_dyn.t[1] track = create_track(0.3) figure() @@ -62,19 +61,13 @@ function eval_sim() legend(["a_f","a_r"]) figure() - plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est.z[:,1],est.z[:,2],"-") + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est_dyn.z[:,1],est_dyn.z[:,2],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) title("x-y-view") axis("equal") legend(["Real state","GPS meas","estimate"]) - figure() - plot(est_dyn.t,est_dyn.z,"-*",est.t,est.z,"--",z.t,z.z,"-") - title("Dyn. est. -*, est. --, real -") - grid() - legend(["x","y","v_x","v_y","psi","psi_dot","x","y","psi","v","x","y","v_x","v_y","psi","psi_dot","d_f"]) - figure() title("Comparison of psi") plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],est_dyn.t,est_dyn.z[:,5:6],"-*") @@ -120,20 +113,40 @@ function eval_LMPC() sol_u = d_lmpc["sol_u"] cost = d_lmpc["cost"] curv = d_lmpc["curv"] - mpcCoeff = d_lmpc["mpcCoeff"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] s_start = d_lmpc["s_start"] est = d_sim["estimate"] + est_dyn = d_sim["estimate_dyn"] imu_meas = d_sim["imu_meas"] gps_meas = d_sim["gps_meas"] z = d_sim["z"] - cmd_log = d_sim["cmd_log"] + cmd_log = d_sim["cmd_log"] # this is the command how it was received by the simulator t0 = t[1] + figure() + ax1=subplot(311) + plot(z.t-t0,z.z[:,3],"--",est_dyn.t-t0,est_dyn.z[:,3],".",t-t0,state[:,1],"-o") + legend(["x_dot_real","x_dot_est","x_dot_MPC"]) + grid("on") + xlabel("t [s]") + ylabel("v_x [m/s]") + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*") + legend(["a","d_f","a","d_f"]) + grid("on") + subplot(313,sharex=ax1) + plot(t-t0,c_Vx) + grid("on") + legend(["1","2","3"]) + figure() c = zeros(size(curv,1),1) for i=1:size(curv,1) @@ -194,8 +207,8 @@ function eval_LMPC() # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") # end figure() - plot(oldTraj[:,1,1,1],oldTraj[:,2:4,1,1],"-o") - legend(["e_y","e_psi","v"]) + plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-o") + legend(["v_x","v_y","psiDot","ePsi","eY"]) grid(1) figure() @@ -204,8 +217,8 @@ function eval_LMPC() figure() ax1=subplot(211) - plot(t-t0,state,z.t-t0,z.z[:,1:2]) - legend(["s","e_y","e_psi","v","x","y"]) + plot(t-t0,state,z.t-t0,z.z[:,3:6],"--") + legend(["v_x","v_y","psiDot","ePsi","eY","s","real_v_x","real_v_y","real_psi","real_psiDot"]) grid(1) subplot(212,sharex = ax1) plot(t-t0,cost) @@ -374,12 +387,19 @@ function create_track(w) # add_curve(theta,50,-pi/2) # add_curve(theta,28,0.0) - # SIMPLE track - add_curve(theta,50,0) - add_curve(theta,100,-pi) - add_curve(theta,100,0) - add_curve(theta,100,-pi) - add_curve(theta,49,0) + # # SIMPLE track + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) + + # SHORT SIMPLE track + add_curve(theta,10,0) + add_curve(theta,80,-pi) + add_curve(theta,20,0) + add_curve(theta,80,-pi) + add_curve(theta,9,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index f2f4d618..3dd586ee 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -26,19 +26,6 @@ - - - - - - - - - - - - - @@ -46,12 +33,13 @@ - + - + - + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 247620bd..6e304e72 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -79,15 +79,18 @@ function main() log_t = zeros(10000,1) log_state = zeros(10000,6) log_cost = zeros(10000,6) - log_mpcCoeff = Array(MpcCoeff,10000) + log_c_Vx = zeros(10000,3) + log_c_Vy = zeros(10000,4) + log_c_Psi = zeros(10000,3) + log_cmd = zeros(10000,2) log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) - pub = Publisher("ecu", ECU, queue_size=10) + pub = Publisher("ecu", ECU, queue_size=1) # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=10) + s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1) println("Finished initialization.") # Lap parameters @@ -102,13 +105,13 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 24.0 + posInfo.s_target = 12.0#24.0 k = 0 # overall counter for logging # Precompile coeffConstraintCost: - oldTraj.oldTraj[1:buffersize,6,1] = 1:buffersize - oldTraj.oldTraj[1:buffersize,6,2] = 1:buffersize - posInfo.s = 400 + oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) + oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) + posInfo.s = posInfo.s_target/2 posInfo.s_start = 0 coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) oldTraj.oldTraj[1:buffersize,6,1] = zeros(buffersize,1) @@ -129,6 +132,7 @@ function main() # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter + log_t[k+1] = time() zCurr[i,:] = copy(z_est) # update state information (actually predicted by Kalman filter!) posInfo.s = zCurr[i,6] # update position info posInfo.s_start = copy(s_start_update[1]) # use a copy so s_start is not updated during one iteration @@ -191,12 +195,13 @@ function main() solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') end tt = toq() - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] zCurr[i,6] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") + # append new states and inputs to old trajectory oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target @@ -210,8 +215,8 @@ function main() # Logging # --------------------------- k = k + 1 # counter - log_state[k,:] = z_est - log_t[k] = time() + log_state[k,:] = zCurr[i,:] + log_cmd[k+1,:] = uCurr[i,:] # the command is going to be pubished in the next iteration log_sol_u[:,:,k] = mpcSol.u log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst @@ -219,7 +224,10 @@ function main() log_curv[k,:] = trackCoeff.coeffCurvature log_s_start[k] = posInfo.s_start log_state_x[k,:] = x_est - log_mpcCoeff[k] = mpcCoeff + log_c_Vx[k,:] = mpcCoeff.c_Vx + log_c_Vy[k,:] = mpcCoeff.c_Vy + log_c_Psi[k,:] = mpcCoeff.c_Psi + #log_mpcCoeff[k] = copy(mpcCoeff) if lapStatus.currentLap <= 2 log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) else @@ -238,7 +246,8 @@ function main() log_path = "$(homedir())/simulations/output_LMPC.jld" save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, - "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"mpcCoeff",log_mpcCoeff) + "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], + "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:]) println("Exiting LMPC node. Saved data.") end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index b63cc6e8..9f9f6485 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -125,12 +125,19 @@ def create_track(self): # theta = add_curve(theta,50,-pi/2) # theta = add_curve(theta,28,0) - # SIMPLE RACETRACK (smooth curves): length = 24.0m - theta = add_curve(theta,50,0) - theta = add_curve(theta,100,-pi) - theta = add_curve(theta,100,0) - theta = add_curve(theta,100,-pi) - theta = add_curve(theta,49,0) + # # SIMPLE RACETRACK (smooth curves): length = 24.0m + # theta = add_curve(theta,50,0) + # theta = add_curve(theta,100,-pi) + # theta = add_curve(theta,100,0) + # theta = add_curve(theta,100,-pi) + # theta = add_curve(theta,49,0) + + # SHORT SIMPLE RACETRACK (smooth curves): length = 24.0m + theta = add_curve(theta,10,0) + theta = add_curve(theta,80,-pi) + theta = add_curve(theta,20,0) + theta = add_curve(theta,80,-pi) + theta = add_curve(theta,9,0) # SIMPLER RACETRACK (half circles as curves): diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py index b9e30b68..51373c13 100755 --- a/workspace/src/barc/src/Localization_node.py +++ b/workspace/src/barc/src/Localization_node.py @@ -49,7 +49,7 @@ def localization_node(): # topic subscriptions / publications #rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback) rospy.Subscriber('state_estimate_dynamic', Z_DynBkMdl, dyn_state_estimate_callback) - state_pub = rospy.Publisher('pos_info', pos_info, queue_size = 10) + state_pub = rospy.Publisher('pos_info', pos_info, queue_size = 1) # create localization class and trajectory l = Localization() diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 71045209..32eb2fe2 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -87,18 +87,14 @@ function ECU_callback(msg::ECU) cmd_log.z[cmd_log.i,:] = u_current' end -function est_callback(msg::Z_KinBkMdl) - global est_meas - est_meas.i += 1 - est_meas.t[est_meas.i] = time() - est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] -end - function est_dyn_callback(msg::Z_DynBkMdl) - global est_meas_dyn + global est_meas_dyn, est_meas est_meas_dyn.i += 1 est_meas_dyn.t[est_meas_dyn.i] = time() est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x msg.y msg.v_x msg.v_y msg.psi msg.psi_dot] + est_meas.i += 1 + est_meas.t[est_meas.i] = time() + est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v_x] end function main() @@ -114,7 +110,6 @@ function main() l_B = get_param("L_b") # distance from CoG to rear axel s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) - s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=10) z_current = zeros(60000,4) @@ -162,7 +157,7 @@ function main() imu_data = Imu() imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds yaw = z_current[i,3] + 0*(randn()*0.05 + imu_drift) - yaw_dot = (z_current[i,3]-z_current[i-1,3])/dt + yaw_dot = (z_current[i,3]-z_current[i-1,3])/dt + 0.05*randn() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,yaw_dot) if i%2 == 0 @@ -174,8 +169,8 @@ function main() end # GPS measurements - x = round(z_current[i,1]*100 + 0*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 0*randn()*2) + x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 1*randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 25689100..2ba45033 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -70,29 +70,24 @@ function ECU_callback(msg::ECU) end function est_dyn_callback(msg::Z_DynBkMdl) - global est_meas_dyn + global est_meas_dyn, est_meas est_meas_dyn.i += 1 est_meas_dyn.t[est_meas_dyn.i] = time() est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x msg.y msg.v_x msg.v_y msg.psi msg.psi_dot] -end - -function est_callback(msg::Z_KinBkMdl) - global est_meas est_meas.i += 1 est_meas.t[est_meas.i] = time() - est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v] + est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi sqrt((msg.v_x)^2+(msg.v_y)^2)] end function main() # initiate node, set up publisher / subscriber topics init_node("barc_sim") - pub_enc = Publisher("encoder", Encoder, queue_size=10) - pub_gps = Publisher("indoor_gps", Vector3, queue_size=10) - pub_imu = Publisher("imu/data", Imu, queue_size=10) + pub_enc = Publisher("encoder", Encoder, queue_size=1) + pub_gps = Publisher("indoor_gps", Vector3, queue_size=1) + pub_imu = Publisher("imu/data", Imu, queue_size=1) - s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) - s2 = Subscriber("state_estimate", Z_KinBkMdl, est_callback, queue_size=10) - s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=10) + s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) + s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=1) z_current = zeros(60000,7) z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0] @@ -153,22 +148,23 @@ function main() end # IMU measurements - imu_data = Imu() - imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds - yaw = z_current[i,5] + 0*(randn()*0.05 + imu_drift) + imu_data = Imu() + imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds + yaw = z_current[i,5] + 0*(randn()*0.05 + imu_drift) + psiDot = z_current[i,6] + 0.01*randn() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) - imu_data.angular_velocity = Vector3(0,0,z_current[i,6]) + imu_data.angular_velocity = Vector3(0,0,psiDot) if i%2 == 0 imu_meas.i += 1 imu_meas.t[imu_meas.i] = t - imu_meas.z[imu_meas.i,:] = [yaw z_current[i,6]] + imu_meas.z[imu_meas.i,:] = [yaw psiDot] publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end # GPS measurements - x = round(z_current[i,1]*100 + 0*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 0*randn()*2) + x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm + y = round(z_current[i,2]*100 + 1*randn()*2) if i % 7 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t diff --git a/workspace/src/barc/src/observers.py b/workspace/src/barc/src/observers.py index 3da79a87..52113619 100755 --- a/workspace/src/barc/src/observers.py +++ b/workspace/src/barc/src/observers.py @@ -85,8 +85,7 @@ def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" """ - - + xDim = mx_k.size # dimension of the state mx_kp1 = f(mx_k, *args) # predict next state A = numerical_jac(f, mx_k, *args) # linearize process model about current state diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 7c4d9928..efb7ea9d 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -19,9 +19,9 @@ from barc.msg import ECU, Encoder, Z_DynBkMdl from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 -from numpy import pi, cos, sin, eye, array, zeros, diag +from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign from observers import kinematicLuembergerObserver, ekf -from system_models import f_DynBkMdl, f_DynBkMdl_exact, h_DynBkMdl +from system_models import f_KinBkMdl_predictive, h_KinBkMdl_predictive from tf import transformations from numpy import unwrap @@ -102,12 +102,12 @@ def enc_callback(data): # update encoder v_x, v_y measurements # only valid for small slip angles, still valid for drift? - v_x_enc = (v_FL + v_FR)/2.0*cos(d_f) + v_x_enc = (v_FL + v_FR)/2.0#*cos(d_f) # update old data n_FL_prev = n_FL n_FR_prev = n_FR - t0 = time.time() + t0 = tf # state estimation node @@ -120,17 +120,17 @@ def state_estimation(): rospy.Subscriber('encoder', Encoder, enc_callback) rospy.Subscriber('ecu', ECU, ecu_callback) rospy.Subscriber('indoor_gps', Vector3, gps_callback) - state_pub = rospy.Publisher('state_estimate_dynamic', Z_DynBkMdl, queue_size = 10) + state_pub = rospy.Publisher('state_estimate_dynamic', Z_DynBkMdl, queue_size = 1) # size 1 -> when there's a newer message the older one is dropped # get vehicle dimension parameters L_f = rospy.get_param("L_a") # distance from CoG to front axel L_r = rospy.get_param("L_b") # distance from CoG to rear axel m = rospy.get_param("m") # mass of vehicle I_z = rospy.get_param("I_z") # moment of inertia about z-axis - vhMdl = (L_f, L_r, m, I_z) + vhMdl = (L_f, L_r) # get encoder parameters - dt_vx = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x + dt_vx = rospy.get_param("state_estimation_dynamic/dt_v_enc") # time interval to compute v_x # get tire model B = rospy.get_param("tire_model/B") @@ -158,13 +158,13 @@ def state_estimation(): t0 = time.time() # estimation variables for Luemberger observer - z_EKF = zeros(12) + z_EKF = zeros(8) # estimation variables for EKF - P = eye(12) # initial dynamics coveriance matrix + P = eye(8) # initial dynamics coveriance matrix #Q = (q_std**2)*eye(6) # process noise coveriance matrixif est_mode==1: - Q = diag([0.01,0.01,0.1,0.0001,1.0,0.001,0,0,0,0,0,0]) # values derived from inspecting P matrix during Kalman filter running - + Q = diag([0.1,0.1,0.1,0.1,0.01,0.01,0.01,0.01]) # values derived from inspecting P matrix during Kalman filter running + if est_mode==1: # use gps, IMU, and encoder R = diag([gps_std,gps_std,psi_std,v_std])**2 elif est_mode==2: # use IMU and encoder only @@ -172,46 +172,44 @@ def state_estimation(): elif est_mode==3: # use gps only R = (gps_std**2)*eye(2) elif est_mode==4: # use gps and angular velocity - R = diag([gps_std,gps_std,ang_v_std,v_std,psi_std])**2 + R = diag([gps_std,gps_std,psi_std,v_std])**2 else: rospy.logerr("No estimation mode selected.") - running = False - #running = True + w_z_f = 0 # filtered w_z (= angular velocity psiDot) while not rospy.is_shutdown(): - # publish state estimate - (x,y,v_x,v_y,psi,psi_dot,x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) = z_EKF # note, r = EKF estimate yaw rate + (x,y,psi,v,x_pred,y_pred,psi_pred,v_pred) = z_EKF # note, r = EKF estimate yaw rate + + # use Kalman values to predict state in 0.1s + dt_pred = 0.15 - # publish information - state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) - #state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) + bta = arctan(L_f/(L_f+L_r)*tan(d_f)) + x_pred = x + dt_pred*( v*cos(psi + bta) ) + y_pred = y + dt_pred*( v*sin(psi + bta) ) + psi_pred = psi + dt_pred*v/L_r*sin(bta) + v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) + v_x_pred = cos(bta)*v_pred + v_y_pred = sin(bta)*v_pred + w_z_f = w_z_f + 0.02*(w_z-w_z_f) + + psi_dot_pred = w_z_f + + #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) + state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) # apply EKF - #if v_x_enc > v_x_min: - if FxR > 0 or running: - running = True - # get measurement - y = array([x_meas,y_meas,w_z,v_x_enc,yaw]) - - # define input - u = array([ d_f, FxR ]) - - # build extra arguments for non-linear function - #F_ext = array([ a0, Ff ]) - args = (u, vhMdl, TrMdl, dt) - - # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_DynBkMdl_exact, z_EKF, P, h_DynBkMdl, y, Q, R, args ) - #print "New Iteration ------" - #print P - - else: - z_EKF[0] = float(x_meas) - z_EKF[1] = float(y_meas) - z_EKF[6] = float(x_meas) # predicted values (these are published) - z_EKF[7] = float(y_meas) - + # get measurement + y = array([x_meas,y_meas,yaw,v_x_enc]) + + # define input + u = array([ d_f, FxR ]) + + # build extra arguments for non-linear function + args = (u, vhMdl, dt) + + # apply EKF and get each state estimate + (z_EKF,P) = ekf(f_KinBkMdl_predictive, z_EKF, P, h_KinBkMdl_predictive, y, Q, R, args ) # wait rate.sleep() diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 745d218a..90fdb516 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -373,16 +373,17 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt): # compute slip angle bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + dt_pred = 0.0 # compute next state x_next = x + dt*( v*cos(psi + bta) ) y_next = y + dt*( v*sin(psi + bta) ) psi_next = psi + dt*v/L_b*sin(bta) v_next = v + dt*(a - 0.63*sign(v)*v**2) - x_next_pred = x_next + 0.1*( v*cos(psi + bta) ) - y_next_pred = y_next + 0.1*( v*sin(psi + bta) ) - psi_next_pred = psi_next + 0.1*v/L_b*sin(bta) - v_next_pred = v_next + 0.1*(a - 0.63*sign(v)*v**2) + x_next_pred = x_next + dt_pred*( v*cos(psi + bta) ) + y_next_pred = y_next + dt_pred*( v*sin(psi + bta) ) + psi_next_pred = psi_next + dt_pred*v/L_b*sin(bta) + v_next_pred = v_next + dt_pred*(a - 0.63*sign(v)*v**2) return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) @@ -420,5 +421,7 @@ def h_KinBkMdl_predictive(x): # [0, 0, 0, 1]]) # For GPS only: C = array([[1, 0, 0, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0, 0, 0]]) + [0, 1, 0, 0, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0, 0, 0], + [0, 0, 0, 1, 0, 0, 0, 0]]) return dot(C, x) From 8871526a833ba45d420f07720b3beac65b08ec39 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 10 Oct 2016 11:08:55 -0700 Subject: [PATCH 048/183] Combined position and ey-epsi-estimation in one node and deleted localization node. Corrected LMPC node (New inputs and states were saved at the wrong point). Working version. --- eval_sim/eval_sim.jl | 2 +- workspace/src/barc/launch/barc_sim.launch | 4 +- workspace/src/barc/src/LMPC_node.jl | 12 +-- workspace/src/barc/src/Localization_node.py | 86 ------------------- workspace/src/barc/src/barc_simulator_dyn.jl | 6 +- .../src/barc/src/state_estimation_DynBkMdl.py | 25 ++++-- 6 files changed, 32 insertions(+), 103 deletions(-) delete mode 100755 workspace/src/barc/src/Localization_node.py diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index b3c70e23..3df53a89 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -50,7 +50,7 @@ function eval_sim() ax1=subplot(311) plot(z.t-t0,z.z,"-o") grid() - legend(["x","y","v_x","v_y","psi","psi_dot","d_f"]) + legend(["x","y","v_x","v_y","psi","psi_dot","a","d_f"]) subplot(312,sharex=ax1) plot(cmd_log.t-t0,cmd_log.z,"-o") grid() diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 3dd586ee..ee60790a 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -34,7 +34,7 @@ - + @@ -43,7 +43,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 6e304e72..6b497961 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -157,12 +157,6 @@ function main() switchLap = true end - # For System ID: Update last 50 measurements - z_ID = circshift(z_ID,-1) - u_ID = circshift(u_ID,-1) - z_ID[end,:] = zCurr[i,:] - u_ID[end,:] = uCurr[i,:] - # ======================================= Calculate input ======================================= println("======================================== NEW ITERATION # $i ========================================") println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") @@ -212,6 +206,12 @@ function main() oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] end + # For System ID: Update last 50 measurements + z_ID = circshift(z_ID,-1) + u_ID = circshift(u_ID,-1) + z_ID[end,:] = zCurr[i,:] + u_ID[end,:] = uCurr[i,:] + # Logging # --------------------------- k = k + 1 # counter diff --git a/workspace/src/barc/src/Localization_node.py b/workspace/src/barc/src/Localization_node.py deleted file mode 100755 index 51373c13..00000000 --- a/workspace/src/barc/src/Localization_node.py +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# -# --------------------------------------------------------------------------- - - -import rospy -import time -import os -from numpy import * -from Localization_helpers import Localization -from barc.msg import Z_KinBkMdl, pos_info, Z_DynBkMdl -import matplotlib.pyplot as plt - -l = 0 -epsi_prev = 0 -running = False - -# State estimate callback function -# def state_estimate_callback(data): -# global l, epsi_prev -# # Set current position and orientation -# l.set_pos(data.x,data.y,data.psi,data.v) -# # Update position and trajectory information -# l.find_s() -# # unwrap epsi -# # l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] -# epsi_prev = l.epsi - -def dyn_state_estimate_callback(data): - global l, epsi_prev, running - if running: - # Set current position and orientation - l.set_pos(data.x,data.y,data.psi,data.v_x,data.v_x,data.v_y,data.psi_dot) # v = v_x - # Update position and trajectory information - l.find_s() - # unwrap epsi - # l.epsi = unwrap(array([epsi_prev,l.epsi]),discont=pi)[1] - epsi_prev = l.epsi - -# localization node -def localization_node(): - global l # localization class - global running - # initialize node - rospy.init_node('localization', anonymous=True) - - # topic subscriptions / publications - #rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback) - rospy.Subscriber('state_estimate_dynamic', Z_DynBkMdl, dyn_state_estimate_callback) - state_pub = rospy.Publisher('pos_info', pos_info, queue_size = 1) - - # create localization class and trajectory - l = Localization() - # l.create_circle(1,100,array([3.2,0.5])) - #l.create_racetrack(2.0,2.0,0.2,array([0.0,-1.0]),0) - l.create_track() - #l.create_track2() - # l.create_ellipse(1.5,0.8,100,array([2.8,1.6])) - l.prepare_trajectory(0.06) - #plt.plot(l.nodes[0,:],l.nodes[1,:],'-o') - #print l.nodes - #plt.show() - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - t0 = time.time() - - while not rospy.is_shutdown(): - running = True - - # publish information - state_pub.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.v_x,l.v_y,l.psi,l.psiDot,l.coeffX.tolist(),l.coeffY.tolist(),l.coeffTheta.tolist(),l.coeffCurvature.tolist()) ) - - # wait - rate.sleep() - - -if __name__ == '__main__': - try: - localization_node() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 2ba45033..707633bf 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -51,7 +51,7 @@ imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) est_meas_dyn = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,6)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,7)) +z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,8)) slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) z_real.t[1] = time() @@ -89,8 +89,8 @@ function main() s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=1) - z_current = zeros(60000,7) - z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0] + z_current = zeros(60000,8) + z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0 0.0] slip_ang = zeros(60000,2) dt = 0.01 diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index efb7ea9d..13e94d3d 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -16,7 +16,8 @@ import rospy import time import os -from barc.msg import ECU, Encoder, Z_DynBkMdl +from Localization_helpers import Localization +from barc.msg import ECU, Encoder, Z_DynBkMdl, pos_info from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign @@ -120,8 +121,9 @@ def state_estimation(): rospy.Subscriber('encoder', Encoder, enc_callback) rospy.Subscriber('ecu', ECU, ecu_callback) rospy.Subscriber('indoor_gps', Vector3, gps_callback) - state_pub = rospy.Publisher('state_estimate_dynamic', Z_DynBkMdl, queue_size = 1) # size 1 -> when there's a newer message the older one is dropped - + state_pub = rospy.Publisher('state_estimate_dynamic', Z_DynBkMdl, queue_size = 1) # size 1 -> when there's a newer message the older one is dropped + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 1) + # get vehicle dimension parameters L_f = rospy.get_param("L_a") # distance from CoG to front axel L_r = rospy.get_param("L_b") # distance from CoG to rear axel @@ -176,13 +178,19 @@ def state_estimation(): else: rospy.logerr("No estimation mode selected.") + # Set up track parameters + l = Localization() + l.create_track() + l.prepare_trajectory(0.06) + + w_z_f = 0 # filtered w_z (= angular velocity psiDot) while not rospy.is_shutdown(): # publish state estimate (x,y,psi,v,x_pred,y_pred,psi_pred,v_pred) = z_EKF # note, r = EKF estimate yaw rate # use Kalman values to predict state in 0.1s - dt_pred = 0.15 + dt_pred = 0.0 bta = arctan(L_f/(L_f+L_r)*tan(d_f)) x_pred = x + dt_pred*( v*cos(psi + bta) ) @@ -191,13 +199,20 @@ def state_estimation(): v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) v_x_pred = cos(bta)*v_pred v_y_pred = sin(bta)*v_pred - w_z_f = w_z_f + 0.02*(w_z-w_z_f) + w_z_f = w_z_f + 0.2*(w_z-w_z_f) psi_dot_pred = w_z_f #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) + # Update track position + l.set_pos(x_pred,y_pred,psi_pred,v_x_pred,v_x_pred,v_y_pred,psi_dot_pred) # v = v_x + l.find_s() + + # and then publish position info + state_pub_pos.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.v_x,l.v_y,l.psi,l.psiDot,l.coeffX.tolist(),l.coeffY.tolist(),l.coeffTheta.tolist(),l.coeffCurvature.tolist()) ) + # apply EKF # get measurement y = array([x_meas,y_meas,yaw,v_x_enc]) From 590e2630d386f7e48fb26b63d45a64953cc7561c Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 10 Oct 2016 17:55:44 -0700 Subject: [PATCH 049/183] Adapted to new submodule, went back to GPS estimation only (no yaw or encoders) --- workspace/src/barc/src/LMPC_lib | 2 +- workspace/src/barc/src/LMPC_node.jl | 7 ++----- .../src/barc/src/state_estimation_DynBkMdl.py | 21 ++++++++++--------- workspace/src/barc/src/system_models.py | 3 +-- 4 files changed, 15 insertions(+), 18 deletions(-) diff --git a/workspace/src/barc/src/LMPC_lib b/workspace/src/barc/src/LMPC_lib index a4a5f043..41e1bb96 160000 --- a/workspace/src/barc/src/LMPC_lib +++ b/workspace/src/barc/src/LMPC_lib @@ -1 +1 @@ -Subproject commit a4a5f043a40e30e68bc82ea637b0116fca8679a5 +Subproject commit 41e1bb964699bf98c6d13ad038001232f1810528 diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 6b497961..b16b12e7 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -47,12 +47,9 @@ function main() mpcParams = MpcParams() mpcParams_pF = MpcParams() # for 1st lap (path following) - z_Init = [1.0, 0.0, 0.0, 0.0, 0.0, 0.0] # xDot needs to be > 0 - z_Init_pF = zeros(4) - InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) - mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff,z_Init) - mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff,z_Init_pF) + mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) z_ID = zeros(50,6) u_ID = zeros(50,2) diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 13e94d3d..33aeff3f 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -22,7 +22,7 @@ from geometry_msgs.msg import Vector3 from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign from observers import kinematicLuembergerObserver, ekf -from system_models import f_KinBkMdl_predictive, h_KinBkMdl_predictive +from system_models import f_KinBkMdl, h_KinBkMdl from tf import transformations from numpy import unwrap @@ -160,12 +160,12 @@ def state_estimation(): t0 = time.time() # estimation variables for Luemberger observer - z_EKF = zeros(8) + z_EKF = zeros(4) # estimation variables for EKF - P = eye(8) # initial dynamics coveriance matrix + P = eye(4) # initial dynamics coveriance matrix #Q = (q_std**2)*eye(6) # process noise coveriance matrixif est_mode==1: - Q = diag([0.1,0.1,0.1,0.1,0.01,0.01,0.01,0.01]) # values derived from inspecting P matrix during Kalman filter running + Q = diag([0.1,0.1,0.1,0.1]) # values derived from inspecting P matrix during Kalman filter running if est_mode==1: # use gps, IMU, and encoder R = diag([gps_std,gps_std,psi_std,v_std])**2 @@ -174,7 +174,7 @@ def state_estimation(): elif est_mode==3: # use gps only R = (gps_std**2)*eye(2) elif est_mode==4: # use gps and angular velocity - R = diag([gps_std,gps_std,psi_std,v_std])**2 + R = diag([gps_std,gps_std])**2 else: rospy.logerr("No estimation mode selected.") @@ -187,10 +187,10 @@ def state_estimation(): w_z_f = 0 # filtered w_z (= angular velocity psiDot) while not rospy.is_shutdown(): # publish state estimate - (x,y,psi,v,x_pred,y_pred,psi_pred,v_pred) = z_EKF # note, r = EKF estimate yaw rate + (x,y,psi,v) = z_EKF # note, r = EKF estimate yaw rate # use Kalman values to predict state in 0.1s - dt_pred = 0.0 + dt_pred = 0.1 bta = arctan(L_f/(L_f+L_r)*tan(d_f)) x_pred = x + dt_pred*( v*cos(psi + bta) ) @@ -199,7 +199,7 @@ def state_estimation(): v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) v_x_pred = cos(bta)*v_pred v_y_pred = sin(bta)*v_pred - w_z_f = w_z_f + 0.2*(w_z-w_z_f) + w_z_f = w_z_f + 0.4*(w_z-w_z_f) psi_dot_pred = w_z_f @@ -215,7 +215,8 @@ def state_estimation(): # apply EKF # get measurement - y = array([x_meas,y_meas,yaw,v_x_enc]) + #y = array([x_meas,y_meas,yaw,v_x_enc]) + y = array([x_meas,y_meas]) # define input u = array([ d_f, FxR ]) @@ -224,7 +225,7 @@ def state_estimation(): args = (u, vhMdl, dt) # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_KinBkMdl_predictive, z_EKF, P, h_KinBkMdl_predictive, y, Q, R, args ) + (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) # wait rate.sleep() diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 90fdb516..2c4f20a5 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -422,6 +422,5 @@ def h_KinBkMdl_predictive(x): # For GPS only: C = array([[1, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0, 0, 0], - [0, 0, 0, 1, 0, 0, 0, 0]]) + [0, 0, 1, 0, 0, 0, 0, 0]]) return dot(C, x) From 3d03726c8ac022ec05720b920210239a10f93c8f Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 10 Oct 2016 18:29:34 -0700 Subject: [PATCH 050/183] Added one-step error logging --- eval_sim/eval_sim.jl | 7 +++++++ workspace/src/barc/src/LMPC_node.jl | 18 ++++++++++++++---- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index 3df53a89..e91d8c1d 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -117,6 +117,7 @@ function eval_LMPC() c_Vy = d_lmpc["c_Vy"] c_Psi = d_lmpc["c_Psi"] cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] @@ -131,6 +132,12 @@ function eval_LMPC() t0 = t[1] + figure() + plot(t-t0,step_diff) + grid("on") + title("One-step-errors") + legend(["xDot","yDot","psiDot","ePsi","eY"]) + figure() ax1=subplot(311) plot(z.t-t0,z.z[:,3],"--",est_dyn.t-t0,est_dyn.z[:,3],".",t-t0,state[:,1],"-o") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index b16b12e7..b8e962d5 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -80,6 +80,7 @@ function main() log_c_Vy = zeros(10000,4) log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) + log_step_diff = zeros(10000,5) log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps # Initialize ROS node and topics @@ -98,12 +99,14 @@ function main() zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information u_final = zeros(2) # contains last input of one lap + step_diff = zeros(5) # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 posInfo.s_target = 12.0#24.0 k = 0 # overall counter for logging + mpcSol.z = zeros(10,4) # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) @@ -129,12 +132,20 @@ function main() # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter - log_t[k+1] = time() zCurr[i,:] = copy(z_est) # update state information (actually predicted by Kalman filter!) posInfo.s = zCurr[i,6] # update position info posInfo.s_start = copy(s_start_update[1]) # use a copy so s_start is not updated during one iteration trackCoeff.coeffCurvature = copy(coeffCurvature_update) + # ============================= Pre-Logging (before solving) ================================ + log_t[k+1] = time() # time is measured *before* solving (more consistent that way) + if lapStatus.currentLap <=2 # find 1-step-error + step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[zCurr[i,1], 0, 0, zCurr[i,4], zCurr[i,5]]).^2 + else + step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 + end + log_step_diff[k+1,:] = step_diff + # ======================================= Lap trigger ======================================= if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... println("Finishing one lap at iteration $i") @@ -155,7 +166,7 @@ function main() end # ======================================= Calculate input ======================================= - println("======================================== NEW ITERATION # $i ========================================") + println("=================================== NEW ITERATION # $i ===================================") println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") println("State Nr. $i = $z_est") println("s = $(posInfo.s)") @@ -224,7 +235,6 @@ function main() log_c_Vx[k,:] = mpcCoeff.c_Vx log_c_Vy[k,:] = mpcCoeff.c_Vy log_c_Psi[k,:] = mpcCoeff.c_Psi - #log_mpcCoeff[k] = copy(mpcCoeff) if lapStatus.currentLap <= 2 log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) else @@ -244,7 +254,7 @@ function main() save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], - "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:]) + "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) println("Exiting LMPC node. Saved data.") end From b50cdf417db84358cf81220b2d2ccbe2235174e3 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 12:07:31 -0700 Subject: [PATCH 051/183] Added launchfile for BARC --- .../src/barc/launch/run_remote_lmpc.launch | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 workspace/src/barc/launch/run_remote_lmpc.launch diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch new file mode 100644 index 00000000..1fb95219 --- /dev/null +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 25030abb48602f355ec29f755e1a24ddf65ba259 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 14:32:46 -0700 Subject: [PATCH 052/183] Added env loader for odroid --- env_loader_odroid.sh | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100755 env_loader_odroid.sh diff --git a/env_loader_odroid.sh b/env_loader_odroid.sh new file mode 100755 index 00000000..3ee19e41 --- /dev/null +++ b/env_loader_odroid.sh @@ -0,0 +1,7 @@ +#!/bin/bash +source /opt/ros/indigo/setup.bash +source ~/barc/workspace/devel/setup.bash +export PATH=$PATH:/home/odroid/julia +export ROS_IP=192.168.100.100 +export ROS_MASTER_URI=http://192.168.100.101:11311 +exec "$@" \ No newline at end of file From 49d5d40d5d51d80a95d0016e4f80457a6f83d913 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 14:48:50 -0700 Subject: [PATCH 053/183] Added env loader for pc --- env_loader_pc.sh | 7 +++++++ workspace/src/barc/launch/run_remote_lmpc.launch | 4 ++-- 2 files changed, 9 insertions(+), 2 deletions(-) create mode 100755 env_loader_pc.sh diff --git a/env_loader_pc.sh b/env_loader_pc.sh new file mode 100755 index 00000000..8514434f --- /dev/null +++ b/env_loader_pc.sh @@ -0,0 +1,7 @@ +#!/bin/bash +source /opt/ros/kinetic/setup.bash +source /home/max/barc/workspace/devel/setup.bash +export PATH=$PATH:/home/max/Downloads/julia-2e358ce975/bin +export ROS_IP=192.168.100.101 +export ROS_MASTER_URI=http://192.168.100.101:11311 +exec "$@" \ No newline at end of file diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 1fb95219..f8471816 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -20,8 +20,8 @@ - - + + From 2119b754ae87c762bcc5df66ae255616971945d1 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 15:58:55 -0700 Subject: [PATCH 054/183] Corrected low-level controller --- .../src/barc/launch/run_remote_lmpc.launch | 6 ++++++ workspace/src/barc/msg/ECU.msg | 4 ++-- workspace/src/barc/src/controller_low_level.py | 18 ++++++++---------- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index f8471816..79b729c5 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -49,4 +49,10 @@ + + + + + + diff --git a/workspace/src/barc/msg/ECU.msg b/workspace/src/barc/msg/ECU.msg index 823acb59..2bffecbf 100644 --- a/workspace/src/barc/msg/ECU.msg +++ b/workspace/src/barc/msg/ECU.msg @@ -6,5 +6,5 @@ # # For modeling and state estimation, motors units are [N], and servo units are [rad] # For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle -float64 motor -float64 servo +float32 motor +float32 servo diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 15fcdda3..6d4bc924 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -30,24 +30,23 @@ str_ang_min = -35 def pwm_converter_callback(msg): - global motor_pwm, servo_pwm, b0 + global motor_pwm, servo_pwm global str_ang_max, str_ang_min # translate from SI units in vehicle model # to pwm angle units (i.e. to send command signal to actuators) # convert desired steering angle to degrees, saturate based on input limits - str_ang = max( min( 180.0/pi*msg.servo, str_ang_max), str_ang_min) - servo_pwm = 92.0558 + 1.8194*str_ang - 0.0104*str_ang**2 + servo_pwm = 91.365 + 105.6*float(msg.servo) # compute motor command FxR = float(msg.motor) if FxR == 0: motor_pwm = 90.0 elif FxR > 0: - motor_pwm = FxR/b0 + 95.0 - else: - motor_pwm = 90.0 + motor_pwm = 94.14 + 2.7678*FxR + else: # motor break / slow down + motor_pwm = 93.5 + 46.73*FxR update_arduino() def neutralize(): @@ -62,14 +61,13 @@ def update_arduino(): ecu_pub.publish(ecu_cmd) def arduino_interface(): - global ecu_pub, b0 + global ecu_pub # launch node, subscribe to motorPWM and servoPWM, publish ecu init_node('arduino_interface') - b0 = get_param("input_gain") - Subscriber('ecu', ECU, pwm_converter_callback, queue_size = 10) - ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 10) + Subscriber('ecu', ECU, pwm_converter_callback, queue_size = 1) + ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1) # Set motor to neutral on shutdown on_shutdown(neutralize) From 23fd5c95dda621f03609e894debf14244acd486e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 16:50:33 -0700 Subject: [PATCH 055/183] Fixed 64/32bit issue, completed launchfile. --- .../src/barc/launch/run_remote_lmpc.launch | 19 +++++++++++++++---- workspace/src/barc/msg/Z_DynBkMdl.msg | 12 ++++++------ workspace/src/barc/src/barc_simulator.jl | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 6 +++--- 4 files changed, 25 insertions(+), 14 deletions(-) diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 79b729c5..4f812a34 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -12,7 +12,6 @@ - @@ -25,7 +24,7 @@ - + @@ -43,12 +42,24 @@ + + + - - + + + + + + + + + + + diff --git a/workspace/src/barc/msg/Z_DynBkMdl.msg b/workspace/src/barc/msg/Z_DynBkMdl.msg index 4dc86872..9b1e9e24 100644 --- a/workspace/src/barc/msg/Z_DynBkMdl.msg +++ b/workspace/src/barc/msg/Z_DynBkMdl.msg @@ -1,6 +1,6 @@ -float32 x -float32 y -float32 v_x -float32 v_y -float32 psi -float32 psi_dot \ No newline at end of file +float64 x +float64 y +float64 v_x +float64 v_y +float64 psi +float64 psi_dot \ No newline at end of file diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index 32eb2fe2..d0294bc5 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -27,7 +27,7 @@ using JLD include("LMPC_lib/simModel.jl") -u_current = zeros(2,1) +u_current = zeros(Float64,2,1) # msg ECU is Float32 ! t = 0 diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 707633bf..36cf7299 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -28,7 +28,7 @@ using JLD include("LMPC_lib/classes.jl") include("LMPC_lib/simModel.jl") -u_current = zeros(2,1) +u_current = zeros(Float64,2) # msg ECU is Float32 ! t = 0 @@ -63,10 +63,10 @@ cmd_log.t[1] = time() function ECU_callback(msg::ECU) global u_current - u_current = [msg.motor, msg.servo] + u_current = convert(Array{Float64,1},[msg.motor, msg.servo]) cmd_log.i += 1 cmd_log.t[cmd_log.i] = time() - cmd_log.z[cmd_log.i,:] = u_current' + cmd_log.z[cmd_log.i,:] = u_current end function est_dyn_callback(msg::Z_DynBkMdl) From 07311dff4eccd7a46749826746548620a031e37e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 17:39:50 -0700 Subject: [PATCH 056/183] Added node to record sensor and command data. --- .../src/barc/launch/run_remote_lmpc.launch | 6 +- workspace/src/barc/src/barc_record.jl | 113 ++++++++++++++++++ 2 files changed, 117 insertions(+), 2 deletions(-) create mode 100755 workspace/src/barc/src/barc_record.jl diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 4f812a34..963fca1a 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -25,9 +25,11 @@ + + - + @@ -59,7 +61,7 @@ - + diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl new file mode 100755 index 00000000..f377df89 --- /dev/null +++ b/workspace/src/barc/src/barc_record.jl @@ -0,0 +1,113 @@ +#!/usr/bin/env julia + +#= + Licensing Information: You are free to use or extend these projects for + education or reserach purposes provided that (1) you retain this notice + and (2) you provide clear attribution to UC Berkeley, including a link + to http://barc-project.com + + Attibution Information: The barc project ROS code-base was developed + at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales + (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed + by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was + based on an open source project by Bruce Wootton +=# + +using RobotOS +@rosimport barc.msg: ECU, Z_DynBkMdl +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using sensor_msgs.msg +using JLD + +include("LMPC_lib/classes.jl") + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data + z::Array{T} # measurement values +end + +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + +buffersize = 60000 +gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,3)) +est_meas_dyn = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) +cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + +gps_meas.t[1] = time() +imu_meas.t[1] = time() +est_meas_dyn.t[1] = time() +cmd_log.t[1] = time() + +function ECU_callback(msg::ECU) + global cmd_log + u_current = convert(Array{Float64,1},[msg.motor, msg.servo]) + cmd_log.i += 1 + cmd_log.t[cmd_log.i] = time() + cmd_log.z[cmd_log.i,:] = u_current +end + +function est_dyn_callback(msg::Z_DynBkMdl) + global est_meas_dyn + est_meas_dyn.i += 1 + est_meas_dyn.t[est_meas_dyn.i] = time() + est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x, msg.y, msg.v_x, msg.v_y, msg.psi, msg.psi_dot] +end + +function IMU_callback(msg::Imu) + global imu_meas + imu_meas.i += 1 + imu_meas.t[imu_meas.i] = time() + imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z] +end + +function GPS_callback(msg::Vector3) + global gps_meas + gps_meas.i += 1 + gps_meas.t[gps_meas.i] = time() + gps_meas.z[gps_meas.i,:] = [msg.x, msg.y, msg.z] +end + +function main() + # initiate node, set up publisher / subscriber topics + init_node("barc_record") + s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) + s2 = Subscriber("imu/data", Imu, IMU_callback, queue_size=1) + s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=1) + s4 = Subscriber("indoor_gps", Vector3, GPS_callback, queue_size=1) + + dt = 0.1 + loop_rate = Rate(1/dt) + + println("Recorder running.") + while ! is_shutdown() + rossleep(loop_rate) + end + + # Clean up buffers + clean_up(gps_meas) + clean_up(est_meas_dyn) + clean_up(imu_meas) + clean_up(cmd_log) + + # Save simulation data to file + log_path = "$(homedir())/simulations/record.jld" + save(log_path,"gps_meas",gps_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log) + println("Exiting node... Saving recorded data to $log_path.") +end + +if ! isinteractive() + main() +end From 0f9c1903ae136da5eae255af6a3f5532528e6e29 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 11 Oct 2016 18:38:14 -0700 Subject: [PATCH 057/183] Added julia functions to analyze recorded data. --- eval_sim/eval_run.jl | 422 ++++++++++++++++++++++++++ workspace/src/barc/src/barc_record.jl | 18 +- 2 files changed, 437 insertions(+), 3 deletions(-) create mode 100644 eval_sim/eval_run.jl diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl new file mode 100644 index 00000000..d9c7e2d1 --- /dev/null +++ b/eval_sim/eval_run.jl @@ -0,0 +1,422 @@ +using JLD +using PyPlot +using HDF5, JLD, ProfileView + +include("../workspace/src/barc/src/LMPC_lib/classes.jl") + +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data + z::Array{T} # measurement values +end + + +const log_path = "$(homedir())/simulations/record.jld" +const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" + + +function simModel(z,u,dt,l_A,l_B) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = z + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + + return zNext +end + +function eval_run() + d = load(log_path) + + est_dyn = d["estimate_dyn"] + imu_meas = d["imu_meas"] + gps_meas = d["gps_meas"] + cmd_log = d["cmd_log"] + + t0 = est_dyn.t[1] + track = create_track(0.3) + + figure() + plot(gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est_dyn.z[:,1],est_dyn.z[:,2],"-") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid(1) + title("x-y-view") + axis("equal") + legend(["GPS meas","estimate"]) + + figure() + title("Comparison of psi") + plot(imu_meas.t,imu_meas.z[:,6],imu_meas.t,imu_meas.z[:,3],"-x",est_dyn.t,est_dyn.z[:,5:6],"-*") + legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot"]) + grid() + + figure() + title("Raw IMU orientation data") + plot(imu_meas.t,imu_meas.z[:,1:3],"--",imu_meas.t,imu_meas.z[:,4:6]) + grid("on") + legend(["w_x","w_y","w_z","roll","pitch","yaw"]) + + figure() + title("Comparison of v") + plot(est_dyn.t,est_dyn.z[:,3:4],"-*") + legend(["est_xDot","est_yDot"]) + grid() + + figure() + title("Comparison of x,y") + plot(est_dyn.t,est_dyn.z[:,1:2],"-*") + legend(["est_x","est_y"]) + grid() + + #figure() + #plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*") + #grid(1) + #legend(["est","est_dyn"]) + + #figure() + #plot(imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) + #grid(1) + #legend(["psi meas","estimate"]) + + figure() + plot(cmd_log.t-t0,cmd_log.z) + legend(["a","d_f"]) + grid() +end + +function eval_LMPC() + d_sim = load(log_path) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + s_start = d_lmpc["s_start"] + est_dyn = d_sim["estimate_dyn"] + imu_meas = d_sim["imu_meas"] + gps_meas = d_sim["gps_meas"] + cmd_log = d_sim["cmd_log"] # this is the command how it was received by the simulator + + t0 = t[1] + + figure() + plot(t-t0,step_diff) + grid("on") + title("One-step-errors") + legend(["xDot","yDot","psiDot","ePsi","eY"]) + + figure() + ax1=subplot(311) + plot(est_dyn.t-t0,est_dyn.z[:,3],".",t-t0,state[:,1],"-o") + legend(["x_dot_est","x_dot_MPC"]) + grid("on") + xlabel("t [s]") + ylabel("v_x [m/s]") + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*") + legend(["a","d_f","a","d_f"]) + grid("on") + subplot(313,sharex=ax1) + plot(t-t0,c_Vx) + grid("on") + legend(["1","2","3"]) + + figure() + c = zeros(size(curv,1),1) + for i=1:size(curv,1) + s = state[i,1] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + end + plot(s_start+state[:,1],c,"-o") + for i=1:2:size(curv,1) + s = sol_z[:,1,i] + c = zeros(size(curv,1),1) + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + plot(s_start[i]+s,c,"-*") + end + title("Curvature over path") + xlabel("Curvilinear abscissa [m]") + ylabel("Curvature") + grid() + + track = create_track(0.3) + figure() + hold(1) + plot(x_est[:,1],x_est[:,2],"-o") + legend(["Estimated position"]) + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + axis("equal") + grid(1) + # for i=1:size(x_est,1) + # #dir = [cos(x_est[i,3]) sin(x_est[i,3])] + # #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] + # #lin = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir] + # #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] + # #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") + # end + for i=1:4:size(x_est,1) + z_pred = zeros(11,4) + z_pred[1,:] = x_est[i,:] + for j=2:11 + z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-*") + end + + # for i=1:size(x_est,1) + # s = 0.4:.1:2.5 + # ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + # x = ss*coeffX[i,:]' + # y = ss*coeffY[i,:]' + # plot(x,y) + # end + + # rg = 100:500 + # figure() + # plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") + # title("Comparison states and prediction") + # legend(["ey","epsi","v"]) + # grid(1) + # for i=100:5:500 + # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") + # end + figure() + plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-o") + legend(["v_x","v_y","psiDot","ePsi","eY"]) + grid(1) + + figure() + plot(s_start+state[:,1],state[:,[2,4]],"*") + grid() + + figure() + ax1=subplot(211) + plot(t-t0,state) + legend(["v_x","v_y","psiDot","ePsi","eY","s"]) + grid(1) + subplot(212,sharex = ax1) + plot(t-t0,cost) + grid(1) + legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) + # figure() + # plot(1:size(curv,1),curv) + # grid() + # title("Polynomial coefficients") + # legend(["1","2","3","4","5","6","7","8","9"]) +end + +function eval_oldTraj(i) + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + plot(oldTraj[:,1,1,i],oldTraj[:,2:4,1,i],"-o",oldTraj[:,1,2,i],oldTraj[:,2:4,2,i],"-*") +end + +function eval_LMPC_coeff(k) + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + s_start = d["s_start"] + + s = sol_z[:,1,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + grid() + title("Position = $(s_start[k] + s[1]), k = $k") + xlabel("s") + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(313) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + grid() + xlabel("s") + ylabel("v") +end + +function anim_LMPC(k1,k2) + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + s_start = d["s_start"] + state = d["state"] + + N = size(sol_z,1)-1 + + for k=k1:k2 + s = sol_z[1:10,1,k] + subplot(211) + plot(s,sol_u[:,1,k],"-o") + ylim([-1,2]) + xlim([0,2]) + grid() + subplot(212) + plot(s,sol_u[:,2,k],"-o") + ylim([-0.5,0.5]) + xlim([0,2]) + grid() + sleep(0.1) + end + + figure() + hold(0) + for k=k1:k2 + s_state = s_start[k:k+N] + state[k:k+N,1] - s_start[k] + s = sol_z[:,1,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,2,k],"-o",s_state,state[k:k+N,2],"-*",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + grid() + title("Position = $(s_start[k] + s[1]), k = $k") + xlabel("s") + xlim([0,3]) + ylim([-0.2,0.2]) + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,3,k],"-o",s_state,state[k:k+N,3],"-*",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + grid() + xlabel("s") + ylabel("e_Psi") + xlim([0,3]) + ylim([-0.2,0.2]) + subplot(313) + plot(s,sol_z[:,4,k],"-o",s_state,state[k:k+N,4],"-*",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + grid() + xlabel("s") + ylabel("v") + xlim([0,3]) + ylim([0,2]) + sleep(0.1) + end +end + +function anim_MPC(z) + figure() + hold(0) + grid(1) + for i=1:size(z,3) + plot(z[:,:,i]) + xlim([1,11]) + ylim([-2,2]) + sleep(0.01) + end +end + +function anim_curv(curv) + s = 0.0:.05:2.0 + figure() + hold(0) + #ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + for i=1:size(curv,1) + c = ss*curv[i,:]' + plot(s,c) + xlim([0,2]) + ylim([-1.5,1.5]) + sleep(0.1) + end +end + + + +function eval_prof() + Profile.clear() + @load "$(homedir())/simulations/profile.jlprof" + ProfileView.view(li, lidict=lidict) +end + +function create_track(w) + x = [0.0] # starting point + y = [0.0] + x_l = [0.0] # starting point + y_l = [w] + x_r = [0.0] # starting point + y_r = [-w] + ds = 0.06 + + theta = [0.0] + + # SOPHISTICATED TRACK + # add_curve(theta,30,0.0) + # add_curve(theta,60,-2*pi/3) + # add_curve(theta,90,pi) + # add_curve(theta,80,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,50,0.0) + # add_curve(theta,40,-pi/4) + # add_curve(theta,30,pi/4) + # add_curve(theta,20,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,25,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,28,0.0) + + # # SIMPLE track + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) + + # SHORT SIMPLE track + add_curve(theta,10,0) + add_curve(theta,80,-pi) + add_curve(theta,20,0) + add_curve(theta,80,-pi) + add_curve(theta,9,0) + + for i=1:length(theta) + push!(x, x[end] + cos(theta[i])*ds) + push!(y, y[end] + sin(theta[i])*ds) + push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) + push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) + push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) + push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) + end + track = cat(2, x, y, x_l, y_l, x_r, y_r) + return track + #plot(x,y,x_l,y_l,x_r,y_r) +end + +function add_curve(theta::Array{Float64}, length::Int64, angle) + d_theta = 0 + curve = 2*sum(1:length/2)+length/2 + for i=0:length-1 + if i < length/2+1 + d_theta = d_theta + angle / curve + else + d_theta = d_theta - angle / curve + end + push!(theta, theta[end] + d_theta) + end +end diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index f377df89..ba9efc5b 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -25,7 +25,7 @@ using geometry_msgs.msg using sensor_msgs.msg using JLD -include("LMPC_lib/classes.jl") +#include("LMPC_lib/classes.jl") # This type contains measurement data (time, values and a counter) type Measurements{T} @@ -42,7 +42,7 @@ end buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,3)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) est_meas_dyn = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) @@ -51,12 +51,21 @@ imu_meas.t[1] = time() est_meas_dyn.t[1] = time() cmd_log.t[1] = time() +function Quat2Euler(q::Array{Float64}) + sol = zeros(Float64,3) + sol[1] = atan2(2*(q[1]*q[2]+q[3]*q[4]),1-2*(q[2]^2+q[3]^2)) + sol[2] = asin(2*(q[1]*q[3]-q[4]*q[2])) + sol[3] = atan2(2*(q[1]*q[4]+q[2]*q[3]),1-2*(q[3]^2+q[4]^2)) + return sol +end + function ECU_callback(msg::ECU) global cmd_log u_current = convert(Array{Float64,1},[msg.motor, msg.servo]) cmd_log.i += 1 cmd_log.t[cmd_log.i] = time() cmd_log.z[cmd_log.i,:] = u_current + nothing end function est_dyn_callback(msg::Z_DynBkMdl) @@ -64,13 +73,15 @@ function est_dyn_callback(msg::Z_DynBkMdl) est_meas_dyn.i += 1 est_meas_dyn.t[est_meas_dyn.i] = time() est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x, msg.y, msg.v_x, msg.v_y, msg.psi, msg.psi_dot] + nothing end function IMU_callback(msg::Imu) global imu_meas imu_meas.i += 1 imu_meas.t[imu_meas.i] = time() - imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z] + imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z;Quat2Euler([msg.orientation.w;msg.orientation.x;msg.orientation.y;msg.orientation.z])]::Array{Float64} + nothing end function GPS_callback(msg::Vector3) @@ -78,6 +89,7 @@ function GPS_callback(msg::Vector3) gps_meas.i += 1 gps_meas.t[gps_meas.i] = time() gps_meas.z[gps_meas.i,:] = [msg.x, msg.y, msg.z] + nothing end function main() From bf66b83a00544fdfc80bf64275786e6cb63ad183 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 12 Oct 2016 08:07:52 -0700 Subject: [PATCH 058/183] Added sensor test launchfile. --- workspace/src/barc/launch/lmpc.launch | 38 ----------- .../src/barc/launch/sensor_test_remote.launch | 65 +++++++++++++++++++ workspace/src/barc/src/barc_record.jl | 2 +- 3 files changed, 66 insertions(+), 39 deletions(-) delete mode 100644 workspace/src/barc/launch/lmpc.launch create mode 100644 workspace/src/barc/launch/sensor_test_remote.launch diff --git a/workspace/src/barc/launch/lmpc.launch b/workspace/src/barc/launch/lmpc.launch deleted file mode 100644 index f61474ef..00000000 --- a/workspace/src/barc/launch/lmpc.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch new file mode 100644 index 00000000..0e5bff2a --- /dev/null +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index ba9efc5b..19b88fb4 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -115,7 +115,7 @@ function main() clean_up(cmd_log) # Save simulation data to file - log_path = "$(homedir())/simulations/record.jld" + log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" save(log_path,"gps_meas",gps_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log) println("Exiting node... Saving recorded data to $log_path.") end From 8dbf111dadc4620955db74905db29f1f17d753a9 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 12 Oct 2016 10:13:56 -0700 Subject: [PATCH 059/183] Fixed bug so python scripts work on odroid as well. --- env_loader_odroid.sh | 1 + eval_sim/eval_run.jl | 8 +++++++- workspace/src/barc/launch/run_remote_lmpc.launch | 2 +- workspace/src/barc/src/barc_record.jl | 2 +- 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/env_loader_odroid.sh b/env_loader_odroid.sh index 3ee19e41..541853ee 100755 --- a/env_loader_odroid.sh +++ b/env_loader_odroid.sh @@ -1,4 +1,5 @@ #!/bin/bash +source ~/barc/scripts/startup.sh source /opt/ros/indigo/setup.bash source ~/barc/workspace/devel/setup.bash export PATH=$PATH:/home/odroid/julia diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl index d9c7e2d1..d48e6e59 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_run.jl @@ -11,7 +11,7 @@ type Measurements{T} end -const log_path = "$(homedir())/simulations/record.jld" +const log_path = "$(homedir())/simulations/record-2016-10-12-09-32-21.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" @@ -51,6 +51,12 @@ function eval_run() axis("equal") legend(["GPS meas","estimate"]) + figure() + plot(gps_meas.t,gps_meas.z/100,"-*",est_dyn.t,est_dyn.z[:,1:2],"-o") + grid(1) + title("GPS comparison") + legend(["x_meas","y_meas","x_est","y_est"]) + figure() title("Comparison of psi") plot(imu_meas.t,imu_meas.z[:,6],imu_meas.t,imu_meas.z[:,3],"-x",est_dyn.t,est_dyn.z[:,5:6],"-*") diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 963fca1a..1ee4e715 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -29,7 +29,7 @@ - + diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 19b88fb4..fc5ba694 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -88,7 +88,7 @@ function GPS_callback(msg::Vector3) global gps_meas gps_meas.i += 1 gps_meas.t[gps_meas.i] = time() - gps_meas.z[gps_meas.i,:] = [msg.x, msg.y, msg.z] + gps_meas.z[gps_meas.i,:] = [msg.x, msg.y] nothing end From 7c97c1ac8d198297b5c6e6e76a5699f9a1947c06 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 12 Oct 2016 11:57:40 -0700 Subject: [PATCH 060/183] Simplified dynamic state estimation: Only position info is published. Adapted evaluation scripts accordingly. --- eval_sim/eval_run.jl | 55 ++++++++++--------- .../src/barc/launch/run_remote_lmpc.launch | 4 +- workspace/src/barc/src/barc_record.jl | 38 ++++++------- .../src/barc/src/state_estimation_DynBkMdl.py | 18 +----- .../marvelmind_indoor_gps/src/indoor_gps.py | 3 +- 5 files changed, 52 insertions(+), 66 deletions(-) diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl index d48e6e59..a067d9d3 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_run.jl @@ -35,16 +35,16 @@ end function eval_run() d = load(log_path) - est_dyn = d["estimate_dyn"] imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] cmd_log = d["cmd_log"] + pos_info = d["pos_info"] - t0 = est_dyn.t[1] + t0 = pos_info.t[1] track = create_track(0.3) figure() - plot(gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est_dyn.z[:,1],est_dyn.z[:,2],"-") + plot(gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",pos_info.z[:,6],pos_info.z[:,7],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) title("x-y-view") @@ -52,14 +52,14 @@ function eval_run() legend(["GPS meas","estimate"]) figure() - plot(gps_meas.t,gps_meas.z/100,"-*",est_dyn.t,est_dyn.z[:,1:2],"-o") + plot(gps_meas.t,gps_meas.z/100,"-*",pos_info.t,pos_info.z[:,6:7],"-o") grid(1) title("GPS comparison") legend(["x_meas","y_meas","x_est","y_est"]) figure() title("Comparison of psi") - plot(imu_meas.t,imu_meas.z[:,6],imu_meas.t,imu_meas.z[:,3],"-x",est_dyn.t,est_dyn.z[:,5:6],"-*") + plot(imu_meas.t,imu_meas.z[:,6],imu_meas.t,imu_meas.z[:,3],"-x",pos_info.t,pos_info.z[:,10:11],"-*") legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot"]) grid() @@ -71,25 +71,20 @@ function eval_run() figure() title("Comparison of v") - plot(est_dyn.t,est_dyn.z[:,3:4],"-*") + plot(pos_info.t,pos_info.z[:,8:9],"-*") legend(["est_xDot","est_yDot"]) grid() figure() - title("Comparison of x,y") - plot(est_dyn.t,est_dyn.z[:,1:2],"-*") - legend(["est_x","est_y"]) - grid() - - #figure() - #plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*") - #grid(1) - #legend(["est","est_dyn"]) - - #figure() - #plot(imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) - #grid(1) - #legend(["psi meas","estimate"]) + subplot(211) + plot(pos_info.t,pos_info.z[:,6:7]) + grid("on") + legend(["x","y"]) + subplot(212) + plot(cmd_log.t,cmd_log.z) + grid("on") + legend(["u","d_f"]) + figure() plot(cmd_log.t-t0,cmd_log.z) @@ -118,10 +113,10 @@ function eval_LMPC() coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] s_start = d_lmpc["s_start"] - est_dyn = d_sim["estimate_dyn"] imu_meas = d_sim["imu_meas"] gps_meas = d_sim["gps_meas"] cmd_log = d_sim["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_sim["pos_info"] t0 = t[1] @@ -133,7 +128,7 @@ function eval_LMPC() figure() ax1=subplot(311) - plot(est_dyn.t-t0,est_dyn.z[:,3],".",t-t0,state[:,1],"-o") + plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-o") legend(["x_dot_est","x_dot_MPC"]) grid("on") xlabel("t [s]") @@ -216,6 +211,7 @@ function eval_LMPC() grid() figure() + title("MPC states and cost") ax1=subplot(211) plot(t-t0,state) legend(["v_x","v_y","psiDot","ePsi","eY","s"]) @@ -224,11 +220,16 @@ function eval_LMPC() plot(t-t0,cost) grid(1) legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) - # figure() - # plot(1:size(curv,1),curv) - # grid() - # title("Polynomial coefficients") - # legend(["1","2","3","4","5","6","7","8","9"]) + + figure() + ax1=subplot(211) + plot(t-t0,state[:,1:5]) + legend(["v_x","v_y","psiDot","ePsi","eY"]) + grid(1) + subplot(212,sharex = ax1) + plot(cmd_log.t-t0,cmd_log.z) + grid(1) + legend(["u","d_f"]) end function eval_oldTraj(i) diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 1ee4e715..975938e7 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -29,7 +29,7 @@ - + @@ -48,7 +48,7 @@ - + diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index fc5ba694..b1f66a2d 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -14,7 +14,7 @@ =# using RobotOS -@rosimport barc.msg: ECU, Z_DynBkMdl +@rosimport barc.msg: ECU, pos_info @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @@ -25,8 +25,6 @@ using geometry_msgs.msg using sensor_msgs.msg using JLD -#include("LMPC_lib/classes.jl") - # This type contains measurement data (time, values and a counter) type Measurements{T} i::Int64 # measurement counter @@ -42,14 +40,14 @@ end buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) -est_meas_dyn = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,6)) +imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,9)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +pos_info_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,11)) gps_meas.t[1] = time() imu_meas.t[1] = time() -est_meas_dyn.t[1] = time() cmd_log.t[1] = time() +pos_info_log.t[1] = time() function Quat2Euler(q::Array{Float64}) sol = zeros(Float64,3) @@ -68,19 +66,13 @@ function ECU_callback(msg::ECU) nothing end -function est_dyn_callback(msg::Z_DynBkMdl) - global est_meas_dyn - est_meas_dyn.i += 1 - est_meas_dyn.t[est_meas_dyn.i] = time() - est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x, msg.y, msg.v_x, msg.v_y, msg.psi, msg.psi_dot] - nothing -end - function IMU_callback(msg::Imu) global imu_meas imu_meas.i += 1 imu_meas.t[imu_meas.i] = time() - imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z;Quat2Euler([msg.orientation.w;msg.orientation.x;msg.orientation.y;msg.orientation.z])]::Array{Float64} + imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z; + Quat2Euler([msg.orientation.w;msg.orientation.x;msg.orientation.y;msg.orientation.z]); + msg.linear_acceleration.x;msg.linear_acceleration.y;msg.linear_acceleration.z]::Array{Float64} nothing end @@ -88,7 +80,15 @@ function GPS_callback(msg::Vector3) global gps_meas gps_meas.i += 1 gps_meas.t[gps_meas.i] = time() - gps_meas.z[gps_meas.i,:] = [msg.x, msg.y] + gps_meas.z[gps_meas.i,:] = [msg.x;msg.y] + nothing +end + +function pos_info_callback(msg::pos_info) + global pos_info_log + pos_info_log.i += 1 + pos_info_log.t[pos_info_log.i] = time() + pos_info_log.z[pos_info_log.i,:] = [msg.s;msg.ey;msg.epsi;msg.v;msg.s_start;msg.x;msg.y;msg.v_x;msg.v_y;msg.psi;msg.psiDot] nothing end @@ -97,8 +97,8 @@ function main() init_node("barc_record") s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) s2 = Subscriber("imu/data", Imu, IMU_callback, queue_size=1) - s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=1) s4 = Subscriber("indoor_gps", Vector3, GPS_callback, queue_size=1) + s5 = Subscriber("pos_info", pos_info, pos_info_callback, queue_size=1) dt = 0.1 loop_rate = Rate(1/dt) @@ -110,13 +110,13 @@ function main() # Clean up buffers clean_up(gps_meas) - clean_up(est_meas_dyn) clean_up(imu_meas) clean_up(cmd_log) + clean_up(pos_info_log) # Save simulation data to file log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" - save(log_path,"gps_meas",gps_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log) + save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"pos_info",pos_info_log) println("Exiting node... Saving recorded data to $log_path.") end diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 33aeff3f..8385f0e7 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -121,25 +121,16 @@ def state_estimation(): rospy.Subscriber('encoder', Encoder, enc_callback) rospy.Subscriber('ecu', ECU, ecu_callback) rospy.Subscriber('indoor_gps', Vector3, gps_callback) - state_pub = rospy.Publisher('state_estimate_dynamic', Z_DynBkMdl, queue_size = 1) # size 1 -> when there's a newer message the older one is dropped state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 1) # get vehicle dimension parameters L_f = rospy.get_param("L_a") # distance from CoG to front axel L_r = rospy.get_param("L_b") # distance from CoG to rear axel - m = rospy.get_param("m") # mass of vehicle - I_z = rospy.get_param("I_z") # moment of inertia about z-axis vhMdl = (L_f, L_r) # get encoder parameters dt_vx = rospy.get_param("state_estimation_dynamic/dt_v_enc") # time interval to compute v_x - # get tire model - B = rospy.get_param("tire_model/B") - C = rospy.get_param("tire_model/C") - mu = rospy.get_param("tire_model/mu") - TrMdl = ([B,C,mu],[B,C,mu]) - # get external force model a0 = rospy.get_param("air_drag_coeff") Ff = rospy.get_param("friction") @@ -183,14 +174,13 @@ def state_estimation(): l.create_track() l.prepare_trajectory(0.06) - w_z_f = 0 # filtered w_z (= angular velocity psiDot) while not rospy.is_shutdown(): # publish state estimate (x,y,psi,v) = z_EKF # note, r = EKF estimate yaw rate # use Kalman values to predict state in 0.1s - dt_pred = 0.1 + dt_pred = 0.0 bta = arctan(L_f/(L_f+L_r)*tan(d_f)) x_pred = x + dt_pred*( v*cos(psi + bta) ) @@ -199,13 +189,10 @@ def state_estimation(): v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) v_x_pred = cos(bta)*v_pred v_y_pred = sin(bta)*v_pred - w_z_f = w_z_f + 0.4*(w_z-w_z_f) + w_z_f = w_z_f + 0.4*(w_z-w_z_f) # simple low pass filter on angular velocity psi_dot_pred = w_z_f - #state_pub.publish( Z_DynBkMdl(x,y,v_x,v_y,psi,psi_dot) ) - state_pub.publish( Z_DynBkMdl(x_pred,y_pred,v_x_pred,v_y_pred,psi_pred,psi_dot_pred) ) - # Update track position l.set_pos(x_pred,y_pred,psi_pred,v_x_pred,v_x_pred,v_y_pred,psi_dot_pred) # v = v_x l.find_s() @@ -215,7 +202,6 @@ def state_estimation(): # apply EKF # get measurement - #y = array([x_meas,y_meas,yaw,v_x_enc]) y = array([x_meas,y_meas]) # define input diff --git a/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py b/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py index b36112e5..a7d47881 100755 --- a/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py +++ b/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py @@ -28,8 +28,7 @@ def indoor_gps_init(serial_device, baudrate): def indoor_gps_data_acq(): # start node rospy.init_node('indoor_gps', anonymous=True) - pub = rospy.Publisher('indoor_gps', Vector3, queue_size = 10) - rospy.loginfo("started node!") + pub = rospy.Publisher('indoor_gps', Vector3, queue_size = 1) # open port port = rospy.get_param("indoor_gps/port") From 9ef7b53cb1b7776dccca06ea028f1bfa683f069b Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 13 Oct 2016 09:32:54 -0700 Subject: [PATCH 061/183] Changed Arduino code to send average velocity (from encoders), adapted Estimator accordingly. Added automatic selection of estimation mode to estimator. Cleaned launch files. --- .../arduino_nano328_node.ino | 104 +++++++++++- workspace/src/barc/launch/barc_sim.launch | 28 +--- .../src/barc/launch/run_remote_lmpc.launch | 23 +-- .../src/barc/launch/sensor_test_remote.launch | 10 -- workspace/src/barc/src/barc_record.jl | 15 +- workspace/src/barc/src/barc_simulator_dyn.jl | 11 +- workspace/src/barc/src/observers.py | 4 +- .../src/barc/src/state_estimation_DynBkMdl.py | 66 +++----- workspace/src/barc/src/system_models.py | 157 +++--------------- 9 files changed, 176 insertions(+), 242 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index c5a70d16..bfde6d2e 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -27,6 +27,9 @@ WARNING: #include #include "Maxbotix.h" #include +#include +#include + /************************************************************************** CAR CLASS DEFINITION (would like to refactor into car.cpp and car.h but can't figure out arduino build process so far) @@ -48,6 +51,10 @@ class Car { int getEncoderFR(); int getEncoderBL(); int getEncoderBR(); + unsigned long getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) // Interrupt service routines void incFR(); void incFL(); @@ -55,6 +62,7 @@ class Car { void incBL(); void calcThrottle(); void calcSteering(); + float getVelocityEstimate(); private: // Pin assignments const int ENC_FL_PIN = 2; @@ -122,6 +130,24 @@ class Car { int BL_count = 0; int BR_count = 0; + // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) + // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + + volatile unsigned long FL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + + unsigned long FL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long FR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + + // Utility functions uint8_t microseconds2PWM(uint16_t microseconds); float saturateMotor(float x); @@ -168,12 +194,24 @@ barc::ECU rc_inputs; barc::Encoder encoder; barc::Ultrasound ultrasound; +std_msgs::Int32 encoder_dt_FL; //(ADDED BY TOMMI 7JULY2016) +std_msgs::Int32 encoder_dt_FR; //(ADDED BY TOMMI 7JULY2016) +std_msgs::Int32 encoder_dt_BL; //(ADDED BY TOMMI 7JULY2016) +std_msgs::Int32 encoder_dt_BR; //(ADDED BY TOMMI 7JULY2016) +std_msgs::Float32 vel_est; // estimation of current velocity to be published + + ros::NodeHandle nh; -ros::Publisher pub_encoder("encoder", &encoder); +//ros::Publisher pub_encoder("encoder", &encoder); ros::Publisher pub_rc_inputs("rc_inputs", &rc_inputs); ros::Publisher pub_ultrasound("ultrasound", &ultrasound); ros::Subscriber sub_ecu("ecu_pwm", ecuCallback); +ros::Publisher pub_encoder_dt_FL("fl", &encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) +ros::Publisher pub_encoder_dt_FR("fr", &encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) +ros::Publisher pub_encoder_dt_BL("bl", &encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) +ros::Publisher pub_encoder_dt_BR("br", &encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) +ros::Publisher pub_vel_est("vel_est", &vel_est); // vel est publisher // Set up ultrasound sensors @@ -198,9 +236,14 @@ void setup() nh.initNode(); // Publish and subscribe to topics - nh.advertise(pub_encoder); +// nh.advertise(pub_encoder); + nh.advertise(pub_encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) + nh.advertise(pub_encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) + nh.advertise(pub_encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) + nh.advertise(pub_encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) nh.advertise(pub_rc_inputs); nh.advertise(pub_ultrasound); + nh.advertise(pub_vel_est); nh.subscribe(sub_ecu); // Arming ESC, 1 sec delay for arming and ROS @@ -226,7 +269,25 @@ void loop() { encoder.FR = car.getEncoderFR(); encoder.BL = car.getEncoderBL(); encoder.BR = car.getEncoderBR(); - pub_encoder.publish(&encoder); + // pub_encoder.publish(&encoder); + + // pubblish encoder dt + encoder_dt_FL.data = (int32_t) car.getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) + encoder_dt_FR.data = (int32_t) car.getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) + encoder_dt_BL.data = (int32_t) car.getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) + encoder_dt_BR.data = (int32_t) car.getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) + vel_est.data = car.getVelocityEstimate(); + pub_encoder_dt_FL.publish(&encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) + pub_encoder_dt_FR.publish(&encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) + pub_encoder_dt_BL.publish(&encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) + pub_encoder_dt_BR.publish(&encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) + + pub_vel_est.publish(&vel_est); // publish estimated velocity + ////////////////////////////////////////////////!!!! + + + + rc_inputs.motor = car.getRCThrottle(); rc_inputs.servo = car.getRCSteering(); @@ -279,7 +340,7 @@ void Car::initEncoders() { pinMode(ENC_FL_PIN, INPUT_PULLUP); pinMode(ENC_BR_PIN, INPUT_PULLUP); pinMode(ENC_BL_PIN, INPUT_PULLUP); - enableInterrupt(ENC_FR_PIN, incFRCallback, CHANGE); + enableInterrupt(ENC_FR_PIN, incFRCallback, CHANGE); //enables interrupts from Pin ENC_FR_PIN, when signal changes (CHANGE). And it call the function 'incFRCallback' enableInterrupt(ENC_FL_PIN, incFLCallback, CHANGE); enableInterrupt(ENC_BR_PIN, incBRCallback, CHANGE); enableInterrupt(ENC_BL_PIN, incBLCallback, CHANGE); @@ -339,21 +400,29 @@ void Car::calcSteering() { void Car::incFL() { FL_count_shared++; + FL_old_time = FL_new_time; //(ADDED BY TOMMI 7JULY2016) + FL_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FL_FLAG; } void Car::incFR() { FR_count_shared++; + FR_old_time = FR_new_time; //(ADDED BY TOMMI 7JULY2016) + FR_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FR_FLAG; } void Car::incBL() { BL_count_shared++; + BL_old_time = BL_new_time; //(ADDED BY TOMMI 7JULY2016) + BL_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BL_FLAG; } void Car::incBR() { BR_count_shared++; + BR_old_time = BR_new_time; //(ADDED BY TOMMI 7JULY2016) + BR_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BR_FLAG; } @@ -375,15 +444,19 @@ void Car::readAndCopyInputs() { } if(updateFlags & FL_FLAG) { FL_count = FL_count_shared; + FL_DeltaTime = FL_new_time - FL_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & FR_FLAG) { FR_count = FR_count_shared; + FR_DeltaTime = FR_new_time - FR_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & BL_FLAG) { BL_count = BL_count_shared; + BL_DeltaTime = BL_new_time - BL_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & BR_FLAG) { BR_count = BR_count_shared; + BR_DeltaTime = BR_new_time - BR_old_time; //(ADDED BY TOMMI 7JULY2016) } // clear shared update flags and turn interrupts back on updateFlagsShared = 0; @@ -410,3 +483,26 @@ int Car::getEncoderBL() { int Car::getEncoderBR() { return BR_count; } + + +unsigned long Car::getEncoder_dTime_FL() { //(ADDED BY TOMMI 7JULY2016) + return FL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_FR() { //(ADDED BY TOMMI 7JULY2016) + return FR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_BL() { //(ADDED BY TOMMI 7JULY2016) + return BL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_BR() { //(ADDED BY TOMMI 7JULY2016) + return BR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) + +float Car::getVelocityEstimate() { + if(FL_DeltaTime > 0 && FR_DeltaTime > 0) { + return 2.0*3.141593*0.036/4.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime)*1000000.0/2.0; // calculate current speed in m/s + } + else { + return 0.0; + } +} \ No newline at end of file diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index ee60790a..13f24f66 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -7,44 +7,22 @@ - - - - - - - - - - - - - - - - - - - + - + - + - - - - diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 975938e7..3ae5ecd1 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -6,18 +6,6 @@ - - - - - - - - - - - - @@ -30,18 +18,15 @@ - - - + - - - + + + - diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index 0e5bff2a..97f9164a 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -6,15 +6,6 @@ - - - - - - - - - @@ -41,7 +32,6 @@ - diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index b1f66a2d..929305a0 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -18,6 +18,7 @@ using RobotOS @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu +@rosimport std_msgs.msg: Float32 rostypegen() using barc.msg using data_service.msg @@ -42,12 +43,14 @@ buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,9)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) +vel_est_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) pos_info_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,11)) gps_meas.t[1] = time() imu_meas.t[1] = time() cmd_log.t[1] = time() pos_info_log.t[1] = time() +vel_est_log.t[1] = time() function Quat2Euler(q::Array{Float64}) sol = zeros(Float64,3) @@ -92,6 +95,14 @@ function pos_info_callback(msg::pos_info) nothing end +function vel_est_callback(msg::Float32Msg) + global vel_est_log + vel_est_log.i += 1 + vel_est_log.t[vel_est_log.i] = time() + vel_est_log.z[vel_est_log.i] = msg.data + nothing +end + function main() # initiate node, set up publisher / subscriber topics init_node("barc_record") @@ -99,6 +110,7 @@ function main() s2 = Subscriber("imu/data", Imu, IMU_callback, queue_size=1) s4 = Subscriber("indoor_gps", Vector3, GPS_callback, queue_size=1) s5 = Subscriber("pos_info", pos_info, pos_info_callback, queue_size=1) + s6 = Subscriber("vel_est", Float32Msg, vel_est_callback, queue_size=1) dt = 0.1 loop_rate = Rate(1/dt) @@ -113,10 +125,11 @@ function main() clean_up(imu_meas) clean_up(cmd_log) clean_up(pos_info_log) + clean_up(vel_est_log) # Save simulation data to file log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" - save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"pos_info",pos_info_log) + save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"pos_info",pos_info_log,"vel_est",vel_est_log) println("Exiting node... Saving recorded data to $log_path.") end diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 36cf7299..7edcb19c 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -18,11 +18,13 @@ using RobotOS @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu +@rosimport std_msgs.msg: Float32 rostypegen() using barc.msg using data_service.msg using geometry_msgs.msg using sensor_msgs.msg +using std_msgs.msg using JLD include("LMPC_lib/classes.jl") @@ -85,6 +87,7 @@ function main() pub_enc = Publisher("encoder", Encoder, queue_size=1) pub_gps = Publisher("indoor_gps", Vector3, queue_size=1) pub_imu = Publisher("imu/data", Imu, queue_size=1) + pub_vel = Publisher("vel_est", Float32Msg, queue_size=1) s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=1) @@ -123,6 +126,8 @@ function main() modelParams.I_z = 0.24 println("Publishing sensor information. Simulator running.") + imu_data = Imu() + vel_est = Float32Msg() while ! is_shutdown() t = time() @@ -148,18 +153,22 @@ function main() end # IMU measurements - imu_data = Imu() imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds yaw = z_current[i,5] + 0*(randn()*0.05 + imu_drift) psiDot = z_current[i,6] + 0.01*randn() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) + + # Velocity measurement + vel_est.data = convert(Float32,norm(z_current[i,3:4])) if i%2 == 0 imu_meas.i += 1 imu_meas.t[imu_meas.i] = t imu_meas.z[imu_meas.i,:] = [yaw psiDot] publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) + publish(pub_vel, vel_est) + end # GPS measurements diff --git a/workspace/src/barc/src/observers.py b/workspace/src/barc/src/observers.py index 52113619..ace3bd0f 100755 --- a/workspace/src/barc/src/observers.py +++ b/workspace/src/barc/src/observers.py @@ -90,8 +90,8 @@ def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): mx_kp1 = f(mx_k, *args) # predict next state A = numerical_jac(f, mx_k, *args) # linearize process model about current state P_kp1 = dot(dot(A,P_k),A.T) + Q # proprogate variance - my_kp1 = h(mx_kp1) # predict future output - H = numerical_jac(h, mx_kp1) # linearize measurement model about predicted next state + my_kp1 = h(mx_kp1, *args) # predict future output + H = numerical_jac(h, mx_kp1, *args) # linearize measurement model about predicted next state P12 = dot(P_kp1, H.T) # cross covariance K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain mx_kp1 = mx_kp1 + dot(K,(y_kp1 - my_kp1)) # state estimate diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 8385f0e7..3036f0d4 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -20,6 +20,7 @@ from barc.msg import ECU, Encoder, Z_DynBkMdl, pos_info from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 +from std_msgs.msg import Float32 from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign from observers import kinematicLuembergerObserver, ekf from system_models import f_KinBkMdl, h_KinBkMdl @@ -47,6 +48,8 @@ x_meas = 0 y_meas = 0 +vel_est = 0 + # ecu command update def ecu_callback(data): global FxR, d_f @@ -82,34 +85,9 @@ def imu_callback(data): a_y = data.linear_acceleration.y a_z = data.linear_acceleration.z -# encoder measurement update -def enc_callback(data): - global v_x_enc, d_f, t0 - global n_FL, n_FR, n_FL_prev, n_FR_prev - - n_FL = data.FL - n_FR = data.FR - - # compute time elapsed - tf = time.time() - dt = tf - t0 - - # if enough time elapse has elapsed, estimate v_x - dt_min = 0.20 - if dt >= dt_min: - # compute speed : speed = distance / time - v_FL = float(n_FL- n_FL_prev)*dx_qrt/dt - v_FR = float(n_FR- n_FR_prev)*dx_qrt/dt - - # update encoder v_x, v_y measurements - # only valid for small slip angles, still valid for drift? - v_x_enc = (v_FL + v_FR)/2.0#*cos(d_f) - - # update old data - n_FL_prev = n_FL - n_FR_prev = n_FR - t0 = tf - +def vel_est_callback(data): + global vel_est + vel_est = data.data # state estimation node def state_estimation(): @@ -118,7 +96,7 @@ def state_estimation(): # topic subscriptions / publications rospy.Subscriber('imu/data', Imu, imu_callback) - rospy.Subscriber('encoder', Encoder, enc_callback) + rospy.Subscriber('vel_est', Float32, vel_est_callback) rospy.Subscriber('ecu', ECU, ecu_callback) rospy.Subscriber('indoor_gps', Vector3, gps_callback) state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 1) @@ -128,20 +106,12 @@ def state_estimation(): L_r = rospy.get_param("L_b") # distance from CoG to rear axel vhMdl = (L_f, L_r) - # get encoder parameters - dt_vx = rospy.get_param("state_estimation_dynamic/dt_v_enc") # time interval to compute v_x - - # get external force model - a0 = rospy.get_param("air_drag_coeff") - Ff = rospy.get_param("friction") - # get EKF observer properties q_std = rospy.get_param("state_estimation_dynamic/q_std") # std of process noise psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation_dynamic/v_std") + v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - ang_v_std = rospy.get_param("state_estimation_dynamic/ang_v_std") # std of gps measurements - v_x_min = rospy.get_param("state_estimation_dynamic/v_x_min") # minimum velociy before using EKF + ang_v_std = rospy.get_param("state_estimation_dynamic/ang_v_std") # std of angular velocity measurements est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode # set node rate @@ -164,8 +134,8 @@ def state_estimation(): R = diag([psi_std,v_std])**2 elif est_mode==3: # use gps only R = (gps_std**2)*eye(2) - elif est_mode==4: # use gps and angular velocity - R = diag([gps_std,gps_std])**2 + elif est_mode==4: # use gps and encoder + R = diag([gps_std,gps_std,v_std])**2 else: rospy.logerr("No estimation mode selected.") @@ -202,13 +172,23 @@ def state_estimation(): # apply EKF # get measurement - y = array([x_meas,y_meas]) + if est_mode==1: + y = array([x_meas,y_meas,yaw,vel_est]) + if est_mode==2: + y = array([yaw,vel_est]) + if est_mode==3: + y = array([x_meas,y_meas]) + elif est_mode==4: + y = array([x_meas,y_meas,vel_est]) + else: + print("Wrong estimation mode specified.") + # define input u = array([ d_f, FxR ]) # build extra arguments for non-linear function - args = (u, vhMdl, dt) + args = (u, vhMdl, dt, est_mode) # apply EKF and get each state estimate (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 2c4f20a5..dc0793b3 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -188,7 +188,7 @@ def f_pacejka(trMdl, alpha): return d*sin(c*arctan(b*alpha)) -def f_KinBkMdl(z,u,vhMdl, dt): +def f_KinBkMdl(z,u,vhMdl, dt, est_mode): """ process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] @@ -219,133 +219,7 @@ def f_KinBkMdl(z,u,vhMdl, dt): return array([x_next, y_next, psi_next, v_next]) -def pacejka(a): - B = 1.0 - C = 1.0 - mu = 0.8 - m = 1.98 - g = 9.81 - D = mu * m * g/2 - D = D*10 - - C_alpha_f = D*sin(C*arctan(B*a)) - return C_alpha_f - -def f_DynBkMdl_exact(z,u,vhMdl,trMdl,dt): - zNext = z - dtn = dt / 10 - for i in range(0,10): - zNext = f_DynBkMdl(zNext,u,vhMdl,trMdl,dtn) - - return zNext - -def f_DynBkMdl(z,u,vhMdl,trMdl,dt): - x_I = z[0] - y_I = z[1] - v_x = z[2] - v_y = z[3] - psi = z[4] - psi_dot = z[5] - x_I_pred = z[6] - y_I_pred = z[7] - v_x_pred = z[8] - v_y_pred = z[9] - psi_pred = z[10] - psi_dot_pred = z[11] - - d_f = u[0] - a = u[1] - - dt_pred = 0.15 - - # extract parameters - (L_f,L_r,m,I_z) = vhMdl - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = 0 - a_R = 0 - if abs(v_x) > 0.1: - a_F = arctan((v_y + L_f*psi_dot)/v_x) - d_f - a_R = arctan((v_y - L_r*psi_dot)/v_x) - - FyF = -pacejka(a_F) - FyR = -pacejka(a_R) - - #a_F = 0 # experimental: set all forces to zero - #a_R = 0 - #FyF = 0 - #FyR = 0 - - if abs(a_F) > 30.0/180.0*3.1416 or abs(a_R) > 30.0/180.0*3.1416: - print("WARNING: Large slip angles in estimation: a_F = %f, a_R = %f"%(a_F,a_R)) - - # compute next state - x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) - y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) - v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) - v_y_next = v_y + dt * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) - psi_next = psi + dt * (psi_dot) - psi_dot_next = psi_dot + dt * (2/I_z*(L_f*FyF - L_r*FyR)) - - x_I_next_pred = x_I + dt_pred * (cos(psi)*v_x - sin(psi)*v_y) - y_I_next_pred = y_I + dt_pred * (sin(psi)*v_x + cos(psi)*v_y) - v_x_next_pred = v_x + dt_pred * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) - v_y_next_pred = v_y + dt_pred * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) - psi_next_pred = psi + dt_pred * (psi_dot) - psi_dot_next_pred = psi_dot + dt_pred * (2/I_z*(L_f*FyF - L_r*FyR)) - - return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next,x_I_next_pred,y_I_next_pred,v_x_next_pred,v_y_next_pred,psi_next_pred,psi_dot_next_pred]) - -def f_DynBkMdl_Kin(z,u,vhMdl,trMdl,dt): - x_I = z[0] - y_I = z[1] - v_x = z[2] - v_y = z[3] - psi = z[4] - psi_dot = z[5] - x_I_pred = z[6] - y_I_pred = z[7] - v_x_pred = z[8] - v_y_pred = z[9] - psi_pred = z[10] - psi_dot_pred = z[11] - - d_f = u[0] - a = u[1] - - dt_pred = 0.15 - - # extract parameters - (L_f,L_r,m,I_z) = vhMdl - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # compute next state - x_I_next = x_I + dt * (cos(psi)*v_x - sin(psi)*v_y) - y_I_next = y_I + dt * (sin(psi)*v_x + cos(psi)*v_y) - v_x_next = v_x + dt * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) - v_y_next = v_y + dt * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) - psi_next = psi + dt * (psi_dot) - psi_dot_next = psi_dot + dt * (2/I_z*(L_f*FyF - L_r*FyR)) - - x_I_next_pred = x_I + dt_pred * (cos(psi)*v_x - sin(psi)*v_y) - y_I_next_pred = y_I + dt_pred * (sin(psi)*v_x + cos(psi)*v_y) - v_x_next_pred = v_x + dt_pred * (a + v_y*psi_dot - 0.63*v_x**2*sign(v_x)) - v_y_next_pred = v_y + dt_pred * (2/m*(FyF*cos(d_f) + FyR) - psi_dot*v_x) - psi_next_pred = psi + dt_pred * (psi_dot) - psi_dot_next_pred = psi_dot + dt_pred * (2/I_z*(L_f*FyF - L_r*FyR)) - - return array([x_I_next,y_I_next,v_x_next,v_y_next,psi_next,psi_dot_next,x_I_next_pred,y_I_next_pred,v_x_next_pred,v_y_next_pred,psi_next_pred,psi_dot_next_pred]) - - -def f_KinBkMdl_predictive(z,u,vhMdl, dt): +def f_KinBkMdl_predictive(z,u,vhMdl, dt, est_mode): """ process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] @@ -396,18 +270,27 @@ def h_DynBkMdl(x): [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]]) return dot(C, x) -def h_KinBkMdl(x): +def h_KinBkMdl(x, u, vhMdl, dt, est_mode): """ measurement model """ - # For GPS, IMU and encoders: - # C = array([[1, 0, 0, 0], - # [0, 1, 0, 0], - # [0, 0, 1, 0], - # [0, 0, 0, 1]]) - # For GPS only: - C = array([[1, 0, 0, 0], - [0, 1, 0, 0]]) + if est_mode==1: # GPS, IMU, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==2: # IMU, Enc + C = array([[0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==3: # GPS + C = array([[1, 0, 0, 0], + [0, 1, 0, 0]]) + elif est_mode==4: # GPS, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 0, 1]]) + else: + print("Wrong est_mode") return dot(C, x) def h_KinBkMdl_predictive(x): From 9a8b91cfb71dcddc29b1e42f4024e4350f1131cd Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 13 Oct 2016 11:19:37 -0700 Subject: [PATCH 062/183] Fixed arduino code. Somehow only 2 of 4 magnets are recognized by encoders. --- .../arduino_nano328_node.ino | 90 +++++++++---------- 1 file changed, 45 insertions(+), 45 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index bfde6d2e..df250666 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -21,7 +21,7 @@ WARNING: // include libraries #include -#include +//#include #include #include #include @@ -30,7 +30,6 @@ WARNING: #include #include - /************************************************************************** CAR CLASS DEFINITION (would like to refactor into car.cpp and car.h but can't figure out arduino build process so far) **************************************************************************/ @@ -190,27 +189,27 @@ volatile unsigned long t0; // Global message variables // Encoder, RC Inputs, Electronic Control Unit, Ultrasound barc::ECU ecu; -barc::ECU rc_inputs; -barc::Encoder encoder; -barc::Ultrasound ultrasound; - -std_msgs::Int32 encoder_dt_FL; //(ADDED BY TOMMI 7JULY2016) -std_msgs::Int32 encoder_dt_FR; //(ADDED BY TOMMI 7JULY2016) -std_msgs::Int32 encoder_dt_BL; //(ADDED BY TOMMI 7JULY2016) -std_msgs::Int32 encoder_dt_BR; //(ADDED BY TOMMI 7JULY2016) +//barc::ECU rc_inputs; +//barc::Encoder encoder; +//barc::Ultrasound ultrasound; + +//std_msgs::Int32 encoder_dt_FL; //(ADDED BY TOMMI 7JULY2016) +//std_msgs::Int32 encoder_dt_FR; //(ADDED BY TOMMI 7JULY2016) +//std_msgs::Int32 encoder_dt_BL; //(ADDED BY TOMMI 7JULY2016) +//std_msgs::Int32 encoder_dt_BR; //(ADDED BY TOMMI 7JULY2016) std_msgs::Float32 vel_est; // estimation of current velocity to be published ros::NodeHandle nh; //ros::Publisher pub_encoder("encoder", &encoder); -ros::Publisher pub_rc_inputs("rc_inputs", &rc_inputs); -ros::Publisher pub_ultrasound("ultrasound", &ultrasound); +//ros::Publisher pub_rc_inputs("rc_inputs", &rc_inputs); +//ros::Publisher pub_ultrasound("ultrasound", &ultrasound); ros::Subscriber sub_ecu("ecu_pwm", ecuCallback); -ros::Publisher pub_encoder_dt_FL("fl", &encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) -ros::Publisher pub_encoder_dt_FR("fr", &encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) -ros::Publisher pub_encoder_dt_BL("bl", &encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) -ros::Publisher pub_encoder_dt_BR("br", &encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) +//ros::Publisher pub_encoder_dt_FL("fl", &encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) +//ros::Publisher pub_encoder_dt_FR("fr", &encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) +//ros::Publisher pub_encoder_dt_BL("bl", &encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) +//ros::Publisher pub_encoder_dt_BR("br", &encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) ros::Publisher pub_vel_est("vel_est", &vel_est); // vel est publisher @@ -237,12 +236,12 @@ void setup() // Publish and subscribe to topics // nh.advertise(pub_encoder); - nh.advertise(pub_encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) - nh.advertise(pub_encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) - nh.advertise(pub_encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) - nh.advertise(pub_encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) - nh.advertise(pub_rc_inputs); - nh.advertise(pub_ultrasound); + //nh.advertise(pub_encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_rc_inputs); + //nh.advertise(pub_ultrasound); nh.advertise(pub_vel_est); nh.subscribe(sub_ecu); @@ -265,22 +264,22 @@ void loop() { // TODO make encoder and rc_inputs private properties on car? Then // car.publishEncoder(); and car.publishRCInputs(); - encoder.FL = car.getEncoderFL(); - encoder.FR = car.getEncoderFR(); - encoder.BL = car.getEncoderBL(); - encoder.BR = car.getEncoderBR(); + //encoder.FL = car.getEncoderFL(); + //encoder.FR = car.getEncoderFR(); + //encoder.BL = car.getEncoderBL(); + //encoder.BR = car.getEncoderBR(); // pub_encoder.publish(&encoder); // pubblish encoder dt - encoder_dt_FL.data = (int32_t) car.getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) - encoder_dt_FR.data = (int32_t) car.getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) - encoder_dt_BL.data = (int32_t) car.getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) - encoder_dt_BR.data = (int32_t) car.getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) + //encoder_dt_FL.data = (int32_t) car.getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) + //encoder_dt_FR.data = (int32_t) car.getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) + //encoder_dt_BL.data = (int32_t) car.getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) + //encoder_dt_BR.data = (int32_t) car.getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) vel_est.data = car.getVelocityEstimate(); - pub_encoder_dt_FL.publish(&encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) - pub_encoder_dt_FR.publish(&encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) - pub_encoder_dt_BL.publish(&encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) - pub_encoder_dt_BR.publish(&encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) + //pub_encoder_dt_FL.publish(&encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) + //pub_encoder_dt_FR.publish(&encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) + //pub_encoder_dt_BL.publish(&encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) + //pub_encoder_dt_BR.publish(&encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) pub_vel_est.publish(&vel_est); // publish estimated velocity ////////////////////////////////////////////////!!!! @@ -289,9 +288,9 @@ void loop() { - rc_inputs.motor = car.getRCThrottle(); - rc_inputs.servo = car.getRCSteering(); - pub_rc_inputs.publish(&rc_inputs); + //rc_inputs.motor = car.getRCThrottle(); + //rc_inputs.servo = car.getRCSteering(); + //pub_rc_inputs.publish(&rc_inputs); // publish ultra-sound measurement /* @@ -340,10 +339,10 @@ void Car::initEncoders() { pinMode(ENC_FL_PIN, INPUT_PULLUP); pinMode(ENC_BR_PIN, INPUT_PULLUP); pinMode(ENC_BL_PIN, INPUT_PULLUP); - enableInterrupt(ENC_FR_PIN, incFRCallback, CHANGE); //enables interrupts from Pin ENC_FR_PIN, when signal changes (CHANGE). And it call the function 'incFRCallback' - enableInterrupt(ENC_FL_PIN, incFLCallback, CHANGE); - enableInterrupt(ENC_BR_PIN, incBRCallback, CHANGE); - enableInterrupt(ENC_BL_PIN, incBLCallback, CHANGE); + enableInterrupt(ENC_FR_PIN, incFRCallback, FALLING); //enables interrupts from Pin ENC_FR_PIN, when signal changes (CHANGE). And it call the function 'incFRCallback' + enableInterrupt(ENC_FL_PIN, incFLCallback, FALLING); + enableInterrupt(ENC_BR_PIN, incBRCallback, FALLING); + enableInterrupt(ENC_BL_PIN, incBLCallback, FALLING); } void Car::initRCInput() { @@ -490,7 +489,7 @@ unsigned long Car::getEncoder_dTime_FL() { //(ADDE } //(ADDED BY TOMMI 7JULY2016) unsigned long Car::getEncoder_dTime_FR() { //(ADDED BY TOMMI 7JULY2016) return FR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) -} //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016)+ unsigned long Car::getEncoder_dTime_BL() { //(ADDED BY TOMMI 7JULY2016) return BL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) } //(ADDED BY TOMMI 7JULY2016) @@ -500,9 +499,10 @@ unsigned long Car::getEncoder_dTime_BR() { //(ADDE float Car::getVelocityEstimate() { if(FL_DeltaTime > 0 && FR_DeltaTime > 0) { - return 2.0*3.141593*0.036/4.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime)*1000000.0/2.0; // calculate current speed in m/s - } + return 2.0*3.141593*0.036/2.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime)*1000000.0/2.0; // calculate current speed in m/s + //return 1.0; +} else { return 0.0; } -} \ No newline at end of file +} From baee219aa80416420cbb5fafe0a7fd6a4d5ec3ed Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 13 Oct 2016 18:31:18 -0700 Subject: [PATCH 063/183] Estimator: added option to account for yaw drift. Fixed wrapping error. Adapted evaluation scripts. --- eval_sim/eval_run.jl | 4 +- eval_sim/eval_sim.jl | 19 +- workspace/src/barc/launch/barc_sim.launch | 6 +- workspace/src/barc/src/barc_record.jl | 1 + workspace/src/barc/src/barc_simulator_dyn.jl | 28 +-- .../src/barc/src/state_estimation_DynBkMdl.py | 85 ++++--- workspace/src/barc/src/system_models.py | 211 ++++-------------- 7 files changed, 123 insertions(+), 231 deletions(-) diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl index a067d9d3..ca93247a 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_run.jl @@ -11,7 +11,7 @@ type Measurements{T} end -const log_path = "$(homedir())/simulations/record-2016-10-12-09-32-21.jld" +const log_path = "$(homedir())/simulations/record-2016-10-13-17-33-02.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" @@ -38,7 +38,7 @@ function eval_run() imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] cmd_log = d["cmd_log"] - pos_info = d["pos_info"] + #pos_info = d["pos_info"] t0 = pos_info.t[1] track = create_track(0.3) diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index e91d8c1d..c9ac359f 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -12,6 +12,7 @@ end const log_path = "$(homedir())/simulations/output.jld" +const log_path_record = "$(homedir())/simulations/record-2016-10-13-18-28-41.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" const log_path_profile = "$(homedir())/simulations/profile.jlprof" @@ -35,15 +36,17 @@ end function eval_sim() d = load(log_path) + d2 = load(log_path_record) - est_dyn = d["estimate_dyn"] imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] z = d["z"] cmd_log = d["cmd_log"] slip_a = d["slip_a"] + pos_info = d2["pos_info"] + vel_est = d2["vel_est"] - t0 = est_dyn.t[1] + t0 = pos_info.t[1] track = create_track(0.3) figure() @@ -61,7 +64,7 @@ function eval_sim() legend(["a_f","a_r"]) figure() - plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",est_dyn.z[:,1],est_dyn.z[:,2],"-") + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",pos_info.z[:,6],pos_info.z[:,7],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) title("x-y-view") @@ -70,20 +73,20 @@ function eval_sim() figure() title("Comparison of psi") - plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],est_dyn.t,est_dyn.z[:,5:6],"-*") + plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],pos_info.t,pos_info.z[:,10:11],"-*") legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) grid() figure() title("Comparison of v") - plot(z.t,z.z[:,3:4],est_dyn.t,est_dyn.z[:,3:4],"-*") - legend(["real_xDot","real_yDot","est_xDot","est_yDot"]) + plot(z.t,z.z[:,3:4],pos_info.t,pos_info.z[:,8:9],"-*",vel_est.t,vel_est.z) + legend(["real_xDot","real_yDot","est_xDot","est_yDot","v_x_meas"]) grid() figure() title("Comparison of x,y") - plot(z.t,z.z[:,1:2],est_dyn.t,est_dyn.z[:,1:2],"-*") - legend(["real_x","real_y","est_x","est_y"]) + plot(z.t,z.z[:,1:2],pos_info.t,pos_info.z[:,6:7],"-*",gps_meas.t,gps_meas.z/100) + legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) grid() #figure() diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 13f24f66..088f68e7 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -16,11 +16,11 @@ - + - + @@ -29,5 +29,7 @@ + + diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 929305a0..c8c39854 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -24,6 +24,7 @@ using barc.msg using data_service.msg using geometry_msgs.msg using sensor_msgs.msg +using std_msgs.msg using JLD # This type contains measurement data (time, values and a counter) diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 7edcb19c..fcc9787a 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -50,8 +50,6 @@ end buffersize = 60000 gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) -est_meas_dyn = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,6)) cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,8)) slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) @@ -59,8 +57,6 @@ slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) z_real.t[1] = time() slip_a.t[1] = time() imu_meas.t[1] = time() -est_meas.t[1] = time() -est_meas_dyn.t[1] = time() cmd_log.t[1] = time() function ECU_callback(msg::ECU) @@ -71,16 +67,6 @@ function ECU_callback(msg::ECU) cmd_log.z[cmd_log.i,:] = u_current end -function est_dyn_callback(msg::Z_DynBkMdl) - global est_meas_dyn, est_meas - est_meas_dyn.i += 1 - est_meas_dyn.t[est_meas_dyn.i] = time() - est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x msg.y msg.v_x msg.v_y msg.psi msg.psi_dot] - est_meas.i += 1 - est_meas.t[est_meas.i] = time() - est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi sqrt((msg.v_x)^2+(msg.v_y)^2)] -end - function main() # initiate node, set up publisher / subscriber topics init_node("barc_sim") @@ -90,7 +76,6 @@ function main() pub_vel = Publisher("vel_est", Float32Msg, queue_size=1) s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) - s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=1) z_current = zeros(60000,8) z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0 0.0] @@ -112,7 +97,7 @@ function main() BL = 0 #back left wheel encoder counter BR = 0 #back right wheel encoder counter - imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) + imu_drift = 0.0 # simulates yaw-sensor drift over time (slow sine) modelParams = ModelParams() # modelParams.l_A = copy(get_param("L_a")) # always throws segmentation faults *after* execution!!! ?? @@ -128,6 +113,7 @@ function main() println("Publishing sensor information. Simulator running.") imu_data = Imu() vel_est = Float32Msg() + t0 = time() while ! is_shutdown() t = time() @@ -153,14 +139,14 @@ function main() end # IMU measurements - imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds - yaw = z_current[i,5] + 0*(randn()*0.05 + imu_drift) + imu_drift = (t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds + yaw = z_current[i,5] + randn()*0.05 + imu_drift psiDot = z_current[i,6] + 0.01*randn() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) # Velocity measurement - vel_est.data = convert(Float32,norm(z_current[i,3:4])) + vel_est.data = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) if i%2 == 0 imu_meas.i += 1 imu_meas.t[imu_meas.i] = t @@ -189,8 +175,6 @@ function main() # Clean up buffers clean_up(gps_meas) - clean_up(est_meas) - clean_up(est_meas_dyn) clean_up(imu_meas) clean_up(cmd_log) z_real.z[1:i-1,:] = z_current[1:i-1,:] @@ -202,7 +186,7 @@ function main() # Save simulation data to file log_path = "$(homedir())/simulations/output.jld" - save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) + save(log_path,"gps_meas",gps_meas,"z",z_real,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") #writedlm(log_path,z_current[1:i-1,:]) end diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 3036f0d4..a95d5d70 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -23,7 +23,7 @@ from std_msgs.msg import Float32 from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign from observers import kinematicLuembergerObserver, ekf -from system_models import f_KinBkMdl, h_KinBkMdl +from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_psi_drift, h_KinBkMdl_psi_drift from tf import transformations from numpy import unwrap @@ -33,28 +33,24 @@ # raw measurement variables yaw_prev = 0 -(roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) - -# from encoder -v_x_enc = 0 -t0 = time.time() -n_FL = 0 # counts in the front left tire -n_FR = 0 # counts in the front right tire -n_FL_prev = 0 -n_FR_prev = 0 -r_tire = 0.04 # radius from tire center to perimeter along magnets [m] -dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge +yaw0 = 0 # yaw at t = 0 +yaw = 0 +(roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) x_meas = 0 y_meas = 0 vel_est = 0 +running = False + # ecu command update def ecu_callback(data): - global FxR, d_f + global FxR, d_f, running FxR = data.motor # input motor force [N] d_f = data.servo # input steering angle [rad] + if not running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + running = True # ultrasound gps data def gps_callback(data): @@ -66,16 +62,23 @@ def gps_callback(data): # imu measurement update def imu_callback(data): # units: [rad] and [rad/s] - global roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z - global yaw_prev + global roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z + global yaw_prev, yaw0, yaw # get orientation from quaternion data, and convert to roll, pitch, yaw # extract angular velocity and linear acceleration data ori = data.orientation quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll, pitch, yaw) = transformations.euler_from_quaternion(quaternion) - yaw = unwrap(array([yaw_prev, yaw]), discont = pi)[1] - yaw_prev = yaw + (roll_meas, pitch_meas, yaw_meas) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([yaw_prev,yaw_meas])[1] # get smooth yaw (from beginning) + yaw_prev = yaw # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not running: + yaw0 = yaw # set yaw0 to current yaw + yaw = 0 # and current yaw to zero + else: + yaw = yaw - yaw0 # extract angular velocity and linear acceleration data w_x = data.angular_velocity.x @@ -115,26 +118,35 @@ def state_estimation(): est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode # set node rate - loop_rate = 50 + loop_rate = 25 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) t0 = time.time() - # estimation variables for Luemberger observer - z_EKF = zeros(4) + # settings about psi estimation (use different models accordingly) + psi_drift_active = True + psi_drift = 0 - # estimation variables for EKF - P = eye(4) # initial dynamics coveriance matrix - #Q = (q_std**2)*eye(6) # process noise coveriance matrixif est_mode==1: - Q = diag([0.1,0.1,0.1,0.1]) # values derived from inspecting P matrix during Kalman filter running + if psi_drift_active: + z_EKF = zeros(5) + P = eye(5) # initial dynamics coveriance matrix + Q = diag([5.0,5.0,5.0,10.0,1.0])*dt + else: + z_EKF = zeros(4) + P = eye(4) # initial dynamics coveriance matrix + Q = diag([5.0,5.0,5.0,5.0])*dt if est_mode==1: # use gps, IMU, and encoder + print("Using GPS, IMU and encoders.") R = diag([gps_std,gps_std,psi_std,v_std])**2 elif est_mode==2: # use IMU and encoder only + print("Using IMU and encoders.") R = diag([psi_std,v_std])**2 elif est_mode==3: # use gps only + print("Using GPS.") R = (gps_std**2)*eye(2) elif est_mode==4: # use gps and encoder + print("Using GPS and encoders.") R = diag([gps_std,gps_std,v_std])**2 else: rospy.logerr("No estimation mode selected.") @@ -145,9 +157,14 @@ def state_estimation(): l.prepare_trajectory(0.06) w_z_f = 0 # filtered w_z (= angular velocity psiDot) + psi_prev = 0 # for debugging + while not rospy.is_shutdown(): # publish state estimate - (x,y,psi,v) = z_EKF # note, r = EKF estimate yaw rate + if psi_drift_active: + (x,y,psi,v,psi_drift) = z_EKF # note, r = EKF estimate yaw rate + else: + (x,y,psi,v) = z_EKF # note, r = EKF estimate yaw rate # use Kalman values to predict state in 0.1s dt_pred = 0.0 @@ -174,16 +191,15 @@ def state_estimation(): # get measurement if est_mode==1: y = array([x_meas,y_meas,yaw,vel_est]) - if est_mode==2: + elif est_mode==2: y = array([yaw,vel_est]) - if est_mode==3: + elif est_mode==3: y = array([x_meas,y_meas]) elif est_mode==4: y = array([x_meas,y_meas,vel_est]) else: print("Wrong estimation mode specified.") - # define input u = array([ d_f, FxR ]) @@ -191,7 +207,16 @@ def state_estimation(): args = (u, vhMdl, dt, est_mode) # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) + if psi_drift_active: + (z_EKF,P) = ekf(f_KinBkMdl_psi_drift, z_EKF, P, h_KinBkMdl_psi_drift, y, Q, R, args ) + else: + (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) + + print("yaw = %f, psi = %f"%(yaw,psi_pred)) + if abs(psi_pred-psi_prev) > 0.2: + print("WAAAAAAAAAAAAARNING LARGE PSI DIFFERENCE!!!!!!!!!!!!!!!!!!!******************\n") + print("*****************************************************************************\n") + psi_prev = psi_pred # wait rate.sleep() diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index dc0793b3..a2d461a4 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -17,178 +17,38 @@ from numpy import sign, argmin, sqrt import rospy -# discrete non-linear bicycle model dynamics -def f_2s(z, u, vhMdl, trMdl, dt, v_x): - """ - process model - input: state z at time k, z[k] := [beta[k], r[k]], (i.e. slip angle and yaw rate) - output: state at next time step (k+1) - """ - - # get states / inputs - beta = z[0] - r = z[1] - d_f = u - - # extract parameters - (a,b,m,I_z) = vhMdl - (trMdlFront, trMdlRear) = trMdl - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan(beta + a*r/v_x) - d_f - a_R = arctan(beta - b*r/v_x) - - # compute tire force - FyF = f_pacejka(trMdlFront, a_F) - FyR = f_pacejka(trMdlRear, a_R) - - # compute next state - beta_next = beta + dt*(-r + (1/(m*v_x))*(FyF*cos(d_f)+FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR); - return array([beta_next, r_next]) - -# discrete non-linear bicycle model dynamics -def f_3s(z, u, vhMdl, trMdl, F_ext, dt): +def f_KinBkMdl(z,u,vhMdl, dt, est_mode): """ process model - input: state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) + input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] output: state at next time step z[k+1] """ - - # get states / inputs - v_x = z[0] - v_y = z[1] - r = z[2] - d_f = u[0] - FxR = u[1] - - # extract parameters - (a,b,m,I_z) = vhMdl - (a0, Ff) = F_ext - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # limit force to tire friction circle - if FxR >= mu*Fn: - FxR = mu*Fn - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan((v_y + a*r)/v_x) - d_f - a_R = arctan((v_y - b*r)/v_x) - - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pacejka(TM_param, a_F) - - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pacejka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - FxR**2) - FyR = min(FyR_max, max(-FyR_max, FyR_paj)) - - # compute next state - v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) - v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) - - return array([v_x_next, v_y_next, r_next]) + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) -# discrete non-linear bicycle model dynamics 6-dof -def f_6s(z, u, vhMdl, trMdl, F_ext, dt): - """ - process model - input: state z at time k, z[k] := [X[k], Y[k], phi[k], v_x[k], v_y[k], r[k]]) - output: state at next time step z[k+1] - """ - # get states / inputs - X = z[0] - Y = z[1] - phi = z[2] - v_x = z[3] - v_y = z[4] - r = z[5] + x = z[0] + y = z[1] + psi = z[2] + v = z[3] d_f = u[0] - FxR = u[1] + a = u[1] # extract parameters - (a,b,m,I_z) = vhMdl - (a0, Ff) = F_ext - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # limit force to tire friction circle - if FxR >= mu*Fn: - FxR = mu*Fn - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan((v_y + a*r)/v_x) - d_f - a_R = arctan((v_y - b*r)/v_x) - - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pacejka(TM_param, a_F) + (L_a, L_b) = vhMdl - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pacejka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - FxR**2) - Fy = array([FyR_max, FyR_paj]) - idx = argmin(abs(Fy)) - FyR = Fy[idx] + # compute slip angle + bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) # compute next state - X_next = X + dt*(v_x*cos(phi) - v_y*sin(phi)) - Y_next = Y + dt*(v_x*sin(phi) + v_y*cos(phi)) - phi_next = phi + dt*r - v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) - v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) - - return array([X_next, Y_next, phi_next, v_x_next, v_y_next, r_next]) - -def h_2s(x): - """ - measurement model - state: z := [beta, r], (i.e. slip angle and yaw rate) - output h := r (yaw rate) - """ - C = array([[0, 1]]) - return dot(C, x) - -def h_3s(x): - """ - measurement model - input := state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) - output := [v_x, r] (yaw rate) - """ - C = array([[1, 0, 0], - [0, 0, 1]]) - return dot(C, x) - -def f_pacejka(trMdl, alpha): - """ - f_pacejka = d*sin(c*atan(b*alpha)) - - inputs : - * trMdl := tire model, a list or tuple of parameters (b,c,d) - * alpha := tire slip angle [radians] - outputs : - * Fy := lateral force from tire [Newtons] - """ - (b,c,d) = trMdl - return d*sin(c*arctan(b*alpha)) + x_next = x + dt*( v*cos(psi + bta) ) + y_next = y + dt*( v*sin(psi + bta) ) + psi_next = psi + dt*v/L_b*sin(bta) + v_next = v + dt*(a - 0.63*sign(v)*v**2) + return array([x_next, y_next, psi_next, v_next]) -def f_KinBkMdl(z,u,vhMdl, dt, est_mode): +def f_KinBkMdl_psi_drift(z,u,vhMdl, dt, est_mode): """ process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] @@ -201,6 +61,7 @@ def f_KinBkMdl(z,u,vhMdl, dt, est_mode): y = z[1] psi = z[2] v = z[3] + psi_drift = z[4] d_f = u[0] a = u[1] @@ -216,8 +77,10 @@ def f_KinBkMdl(z,u,vhMdl, dt, est_mode): y_next = y + dt*( v*sin(psi + bta) ) psi_next = psi + dt*v/L_b*sin(bta) v_next = v + dt*(a - 0.63*sign(v)*v**2) + psi_drift_next = psi_drift + + return array([x_next, y_next, psi_next, v_next, psi_drift_next]) - return array([x_next, y_next, psi_next, v_next]) def f_KinBkMdl_predictive(z,u,vhMdl, dt, est_mode): """ @@ -261,15 +124,6 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt, est_mode): return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) -def h_DynBkMdl(x): - # For GPS only: - C = array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]]) - return dot(C, x) - def h_KinBkMdl(x, u, vhMdl, dt, est_mode): """ measurement model @@ -293,6 +147,29 @@ def h_KinBkMdl(x, u, vhMdl, dt, est_mode): print("Wrong est_mode") return dot(C, x) +def h_KinBkMdl_psi_drift(x, u, vhMdl, dt, est_mode): + """ + measurement model + """ + if est_mode==1: # GPS, IMU, Enc + C = array([[1, 0, 0, 0, 0], + [0, 1, 0, 0, 0], + [0, 0, 1, 0, 1], + [0, 0, 0, 1, 0]]) + elif est_mode==2: # IMU, Enc + C = array([[0, 0, 1, 0, 1], + [0, 0, 0, 1, 0]]) + elif est_mode==3: # GPS + C = array([[1, 0, 0, 0, 0], + [0, 1, 0, 0, 0]]) + elif est_mode==4: # GPS, Enc + C = array([[1, 0, 0, 0, 0], + [0, 1, 0, 0, 0], + [0, 0, 0, 1, 0]]) + else: + print("Wrong est_mode") + return dot(C, x) + def h_KinBkMdl_predictive(x): """ measurement model From 6c6ad26ea51dbb3f3d67d00c4c2092c97c7f971e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 14 Oct 2016 09:42:43 -0700 Subject: [PATCH 064/183] Added official marvelmind-package to receive gps-data. Adapted subscribers and simulation accordingly. --- eval_sim/eval_run.jl | 13 +- eval_sim/eval_sim.jl | 6 +- .../src/barc/launch/run_remote_lmpc.launch | 11 +- workspace/src/barc/launch/sensor_test.launch | 22 - .../src/barc/launch/sensor_test_barc.launch | 42 ++ .../src/barc/launch/sensor_test_remote.launch | 16 +- workspace/src/barc/src/barc_record.jl | 8 +- workspace/src/barc/src/barc_simulator_dyn.jl | 14 +- .../src/barc/src/state_estimation_DynBkMdl.py | 9 +- .../src/ros_marvelmind_package/.directory | 4 + workspace/src/ros_marvelmind_package/.hgtags | 1 + .../src/ros_marvelmind_package/CHANGELOG.rst | 14 + .../src/ros_marvelmind_package/CMakeLists.txt | 206 ++++++++ .../include/marvelmind_nav/marvelmind_hedge.h | 85 +++ .../ros_marvelmind_package/msg/hedge_pos.msg | 5 + .../src/ros_marvelmind_package/package.xml | 61 +++ .../src/hedge_rcv_bin.cpp | 128 +++++ .../src/marvelmind_hedge.c | 496 ++++++++++++++++++ .../src/subscriber_test.cpp | 95 ++++ 19 files changed, 1174 insertions(+), 62 deletions(-) delete mode 100644 workspace/src/barc/launch/sensor_test.launch create mode 100644 workspace/src/barc/launch/sensor_test_barc.launch create mode 100644 workspace/src/ros_marvelmind_package/.directory create mode 100644 workspace/src/ros_marvelmind_package/.hgtags create mode 100644 workspace/src/ros_marvelmind_package/CHANGELOG.rst create mode 100644 workspace/src/ros_marvelmind_package/CMakeLists.txt create mode 100644 workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h create mode 100644 workspace/src/ros_marvelmind_package/msg/hedge_pos.msg create mode 100644 workspace/src/ros_marvelmind_package/package.xml create mode 100644 workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp create mode 100644 workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c create mode 100644 workspace/src/ros_marvelmind_package/src/subscriber_test.cpp diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl index ca93247a..3beba75d 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_run.jl @@ -11,7 +11,7 @@ type Measurements{T} end -const log_path = "$(homedir())/simulations/record-2016-10-13-17-33-02.jld" +const log_path = "$(homedir())/simulations/record-2016-10-13-20-03-58.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" @@ -38,13 +38,14 @@ function eval_run() imu_meas = d["imu_meas"] gps_meas = d["gps_meas"] cmd_log = d["cmd_log"] - #pos_info = d["pos_info"] + vel_est = d["vel_est"] + pos_info = d["pos_info"] t0 = pos_info.t[1] track = create_track(0.3) figure() - plot(gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",pos_info.z[:,6],pos_info.z[:,7],"-") + plot(gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) title("x-y-view") @@ -52,7 +53,7 @@ function eval_run() legend(["GPS meas","estimate"]) figure() - plot(gps_meas.t,gps_meas.z/100,"-*",pos_info.t,pos_info.z[:,6:7],"-o") + plot(gps_meas.t,gps_meas.z,"-*",pos_info.t,pos_info.z[:,6:7],"-o") grid(1) title("GPS comparison") legend(["x_meas","y_meas","x_est","y_est"]) @@ -71,8 +72,8 @@ function eval_run() figure() title("Comparison of v") - plot(pos_info.t,pos_info.z[:,8:9],"-*") - legend(["est_xDot","est_yDot"]) + plot(pos_info.t,pos_info.z[:,8:9],"-*",vel_est.t,vel_est.z) + legend(["est_xDot","est_yDot","v_raw"]) grid() figure() diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl index c9ac359f..3c98804c 100644 --- a/eval_sim/eval_sim.jl +++ b/eval_sim/eval_sim.jl @@ -12,7 +12,7 @@ end const log_path = "$(homedir())/simulations/output.jld" -const log_path_record = "$(homedir())/simulations/record-2016-10-13-18-28-41.jld" +const log_path_record = "$(homedir())/simulations/record-2016-10-14-09-22-12.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" const log_path_profile = "$(homedir())/simulations/profile.jlprof" @@ -64,7 +64,7 @@ function eval_sim() legend(["a_f","a_r"]) figure() - plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1]/100,gps_meas.z[:,2]/100,".",pos_info.z[:,6],pos_info.z[:,7],"-") + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) title("x-y-view") @@ -85,7 +85,7 @@ function eval_sim() figure() title("Comparison of x,y") - plot(z.t,z.z[:,1:2],pos_info.t,pos_info.z[:,6:7],"-*",gps_meas.t,gps_meas.z/100) + plot(z.t,z.z[:,1:2],pos_info.t,pos_info.z[:,6:7],"-*",gps_meas.t,gps_meas.z) legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) grid() diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_remote_lmpc.launch index 3ae5ecd1..db63a668 100644 --- a/workspace/src/barc/launch/run_remote_lmpc.launch +++ b/workspace/src/barc/launch/run_remote_lmpc.launch @@ -22,14 +22,14 @@ - - + + - + - + @@ -42,9 +42,8 @@ - + - diff --git a/workspace/src/barc/launch/sensor_test.launch b/workspace/src/barc/launch/sensor_test.launch deleted file mode 100644 index ae8c0c29..00000000 --- a/workspace/src/barc/launch/sensor_test.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/sensor_test_barc.launch b/workspace/src/barc/launch/sensor_test_barc.launch new file mode 100644 index 00000000..eddb31fb --- /dev/null +++ b/workspace/src/barc/launch/sensor_test_barc.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index 97f9164a..7682d1e8 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -6,32 +6,25 @@ - - - - - - - - + - - + + @@ -41,9 +34,8 @@ - + - diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index c8c39854..c259e35c 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -18,6 +18,7 @@ using RobotOS @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu +@rosimport marvelmind_nav.msg: hedge_pos @rosimport std_msgs.msg: Float32 rostypegen() using barc.msg @@ -25,6 +26,7 @@ using data_service.msg using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg +using marvelmind_nav.msg using JLD # This type contains measurement data (time, values and a counter) @@ -80,11 +82,11 @@ function IMU_callback(msg::Imu) nothing end -function GPS_callback(msg::Vector3) +function GPS_callback(msg::hedge_pos) global gps_meas gps_meas.i += 1 gps_meas.t[gps_meas.i] = time() - gps_meas.z[gps_meas.i,:] = [msg.x;msg.y] + gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] nothing end @@ -109,7 +111,7 @@ function main() init_node("barc_record") s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) s2 = Subscriber("imu/data", Imu, IMU_callback, queue_size=1) - s4 = Subscriber("indoor_gps", Vector3, GPS_callback, queue_size=1) + s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, queue_size=1) s5 = Subscriber("pos_info", pos_info, pos_info_callback, queue_size=1) s6 = Subscriber("vel_est", Float32Msg, vel_est_callback, queue_size=1) diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index fcc9787a..a36ee249 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -18,6 +18,7 @@ using RobotOS @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu +@rosimport marvelmind_nav.msg: hedge_pos @rosimport std_msgs.msg: Float32 rostypegen() using barc.msg @@ -25,6 +26,7 @@ using data_service.msg using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg +using marvelmind_nav.msg using JLD include("LMPC_lib/classes.jl") @@ -71,7 +73,7 @@ function main() # initiate node, set up publisher / subscriber topics init_node("barc_sim") pub_enc = Publisher("encoder", Encoder, queue_size=1) - pub_gps = Publisher("indoor_gps", Vector3, queue_size=1) + pub_gps = Publisher("hedge_pos", hedge_pos, queue_size=1) pub_imu = Publisher("imu/data", Imu, queue_size=1) pub_vel = Publisher("vel_est", Float32Msg, queue_size=1) @@ -139,7 +141,7 @@ function main() end # IMU measurements - imu_drift = (t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds + imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) yaw = z_current[i,5] + randn()*0.05 + imu_drift psiDot = z_current[i,6] + 0.01*randn() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) @@ -158,13 +160,13 @@ function main() end # GPS measurements - x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 1*randn()*2) - if i % 7 == 0 + x = round(z_current[i,1] + 0.01*randn()*2,2) # Indoor gps measures in cm + y = round(z_current[i,2] + 0.01*randn()*2,2) + if i % 3 == 0 gps_meas.i += 1 gps_meas.t[gps_meas.i] = t gps_meas.z[gps_meas.i,:] = [x y] - gps_data = Vector3(x,y,0) + gps_data = hedge_pos(0,x,y,0,0) publish(pub_gps, gps_data) end diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index a95d5d70..cb4b253b 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -20,6 +20,7 @@ from barc.msg import ECU, Encoder, Z_DynBkMdl, pos_info from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 +from marvelmind_nav.msg import hedge_pos from std_msgs.msg import Float32 from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign from observers import kinematicLuembergerObserver, ekf @@ -56,8 +57,8 @@ def ecu_callback(data): def gps_callback(data): # units: [rad] and [rad/s] global x_meas, y_meas - x_meas = data.x/100 # data is given in cm - y_meas = data.y/100 + x_meas = data.x_m # data is given in cm + y_meas = data.y_m # imu measurement update def imu_callback(data): @@ -101,7 +102,7 @@ def state_estimation(): rospy.Subscriber('imu/data', Imu, imu_callback) rospy.Subscriber('vel_est', Float32, vel_est_callback) rospy.Subscriber('ecu', ECU, ecu_callback) - rospy.Subscriber('indoor_gps', Vector3, gps_callback) + rospy.Subscriber('hedge_pos', hedge_pos, gps_callback) state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 1) # get vehicle dimension parameters @@ -212,7 +213,7 @@ def state_estimation(): else: (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) - print("yaw = %f, psi = %f"%(yaw,psi_pred)) + #print("yaw = %f, psi = %f"%(yaw,psi_pred)) if abs(psi_pred-psi_prev) > 0.2: print("WAAAAAAAAAAAAARNING LARGE PSI DIFFERENCE!!!!!!!!!!!!!!!!!!!******************\n") print("*****************************************************************************\n") diff --git a/workspace/src/ros_marvelmind_package/.directory b/workspace/src/ros_marvelmind_package/.directory new file mode 100644 index 00000000..87f9bbc9 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/.directory @@ -0,0 +1,4 @@ +[Dolphin] +Timestamp=2016,9,11,21,22,34 +Version=3 +ViewMode=1 diff --git a/workspace/src/ros_marvelmind_package/.hgtags b/workspace/src/ros_marvelmind_package/.hgtags new file mode 100644 index 00000000..6bbd9889 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/.hgtags @@ -0,0 +1 @@ +2a63134d382745f12dca12a47c8197925c1eeb80 1.0.2 diff --git a/workspace/src/ros_marvelmind_package/CHANGELOG.rst b/workspace/src/ros_marvelmind_package/CHANGELOG.rst new file mode 100644 index 00000000..93323d0f --- /dev/null +++ b/workspace/src/ros_marvelmind_package/CHANGELOG.rst @@ -0,0 +1,14 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package marvelmind_nav +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.5 (2016-09-15) +------------------ + +1.0.4 (2016-09-13) +------------------ + +1.0.3 (2016-09-13) +------------------ +* initial commit +* Contributors: smoker77 diff --git a/workspace/src/ros_marvelmind_package/CMakeLists.txt b/workspace/src/ros_marvelmind_package/CMakeLists.txt new file mode 100644 index 00000000..77b706d6 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 2.8.3) +project(marvelmind_nav) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + message_generation + genmsg + visualization_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + DIRECTORY msg + FILES + hedge_pos.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +INCLUDE_DIRS include/${PROJECT_NAME}/ +# LIBRARIES marvelmind_nav +CATKIN_DEPENDS message_runtime +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) +include_directories(include ${roscpp_INCLUDE_DIRS}) + +## Declare a C++ library +add_library(marvelmind_nav + src/marvelmind_hedge.c +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +#add_dependencies(marvelmind_nav ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +add_executable(hedge_rcv_bin +src/hedge_rcv_bin.cpp +src/marvelmind_hedge.c +) + +add_dependencies(hedge_rcv_bin marvelmind_nav_generate_messages_cpp) + +target_link_libraries(hedge_rcv_bin + ${catkin_LIBRARIES} +) + +##################### + +add_executable(subscriber_test +src/subscriber_test.cpp +) + +add_dependencies(subscriber_test marvelmind_nav_generate_messages_cpp) + +target_link_libraries(subscriber_test + ${catkin_LIBRARIES} +) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS marvelmind_nav marvelmind_nav_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +#install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_marvelmind_nav.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h b/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h new file mode 100644 index 00000000..7888296e --- /dev/null +++ b/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h @@ -0,0 +1,85 @@ +#pragma once +#include +#include +#include + +struct PositionValue +{ + uint32_t timestamp; + int16_t x, y, z; + uint8_t flags; +}; + +struct MarvelmindHedge +{ +// serial port device name (physical or USB/virtual). It should be provided as +// an argument: +// /dev/ttyACM0 - typical for Linux / Raspberry Pi +// /dev/tty.usbmodem1451 - typical for Mac OS X + const char * ttyFileName; + +// Baud rate. Should be match to baudrate of hedgehog-beacon +// default: 9600 + uint32_t baudRate; + +// maximum count of measurements of coordinates stored in buffer +// default: 3 + uint8_t maxBufferedPositions; + +// buffer of measurements + struct PositionValue * positionBuffer; + +// verbose flag which activate console output +// default: False + bool verbose; + +// pause flag. If True, class would not read serial data + bool pause; + +// If True, thread would exit from main loop and stop + bool terminationRequired; + +// receiveDataCallback is callback function to recieve data + void (*receiveDataCallback)(struct PositionValue position); + +// private variables + uint8_t lastValuesCount_; + bool haveNewValues_; +#ifdef WIN32 + HANDLE thread_; + CRITICAL_SECTION lock_; +#else + pthread_t thread_; + pthread_mutex_t lock_; +#endif +}; + +#define POSITION_DATAGRAM_HEADER {0xff, 0x47, 0x01, 0x00, 0x10} +#define POSITION_DATAGRAM_HEADER_SIZE 5 + +#pragma pack (push, 1) +struct PositionDatagram_ +{ + uint8_t header[POSITION_DATAGRAM_HEADER_SIZE]; + uint32_t timestamp; + int16_t x, y, z; + uint8_t flags; + uint8_t align[5]; + uint16_t crc; +}; +#pragma pack (pop) + +struct MarvelmindHedge * createMarvelmindHedge (); +void destroyMarvelmindHedge (struct MarvelmindHedge * hedge); +void startMarvelmindHedge (struct MarvelmindHedge * hedge); +void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew); +bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct PositionValue * position); +void stopMarvelmindHedge (struct MarvelmindHedge * hedge); + +#ifdef WIN32 +#define DEFAULT_TTY_FILENAME "\\\\.\\COM3" +#else +#define DEFAULT_TTY_FILENAME "/dev/ttyACM0" +#endif // WIN32 diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg new file mode 100644 index 00000000..84a917dc --- /dev/null +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg @@ -0,0 +1,5 @@ +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +uint8 flags diff --git a/workspace/src/ros_marvelmind_package/package.xml b/workspace/src/ros_marvelmind_package/package.xml new file mode 100644 index 00000000..ab001a4d --- /dev/null +++ b/workspace/src/ros_marvelmind_package/package.xml @@ -0,0 +1,61 @@ + + + marvelmind_nav + 1.0.5 + Marvelmind local navigation system + + + + + + smoker77 + + + + + + BSD + + + + + + + http://marvelmind.com + + + + + + + smoker77 + + + + + + + message_generation + + + + message_runtime + + + catkin + roscpp + rospy + std_msgs + visualization_msgs + roscpp + rospy + std_msgs + visualization_msgs + + + + + + + + diff --git a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp new file mode 100644 index 00000000..3469fac4 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp @@ -0,0 +1,128 @@ +#include +#include +#include +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "marvelmind_nav/hedge_pos.h" +extern "C" +{ +#include "marvelmind_nav/marvelmind_hedge.h" +} + +#include + +#define ROS_NODE_NAME "hedge_rcv_bin" +#define POSITION_TOPIC_NAME "/hedge_pos" + +struct MarvelmindHedge * hedge= NULL; + +static uint32_t hedge_timestamp_prev= 0; +marvelmind_nav::hedge_pos hedge_pos_msg;// message for publishing to ROS topic + +//////////////////////////////////////////////////////////////////////// + +static int hedgeReceivePrepare(int argc, char **argv) +{ + // get port name from command line arguments (if specified) + const char * ttyFileName; + if (argc==2) ttyFileName=argv[1]; + else ttyFileName=DEFAULT_TTY_FILENAME; + + // Init + hedge=createMarvelmindHedge (); + if (hedge==NULL) + { + ROS_INFO ("Error: Unable to create MarvelmindHedge"); + return -1; + } + hedge->ttyFileName=ttyFileName; + hedge->verbose=true; // show errors and warnings + startMarvelmindHedge (hedge); +} + +static bool hedgeReceiveCheck(void) +{ + if (hedge->haveNewValues_) + { + struct PositionValue position; + getPositionFromMarvelmindHedge (hedge, &position); + + hedge_pos_msg.flags= position.flags; + if (hedge_pos_msg.flags&(1<<1))// flag of timestamp format + { + hedge_pos_msg.timestamp_ms= position.timestamp;// msec + } + else + { + hedge_pos_msg.timestamp_ms= position.timestamp*15.625;// alpha-cycles ==> msec + } + + hedge_pos_msg.x_m= position.x/100.0; + hedge_pos_msg.y_m= position.y/100.0; + hedge_pos_msg.z_m= position.z/100.0; + + hedge->haveNewValues_=false; + } +} + +/** + * Node for Marvelmind hedgehog binary streaming data processing + */ +int main(int argc, char **argv) +{ + // initialize ROS node + ros::init(argc, argv, ROS_NODE_NAME); + + // prepare hedgehog data receiver module + hedgeReceivePrepare(argc, argv); + + // ROS node reference + ros::NodeHandle n; + + // Register topic for puplishing messages + ros::Publisher hedge_pos_publisher = n.advertise(POSITION_TOPIC_NAME, 1000); + + // 50 Hz + ros::Rate loop_rate(50); + + // default values for position message + hedge_pos_msg.timestamp_ms = 0; + hedge_pos_msg.x_m = 0.0; + hedge_pos_msg.y_m = 0.0; + hedge_pos_msg.z_m = 0.0; + hedge_pos_msg.flags = (1<<0);// 'data not available' flag + + + while (ros::ok()) + { + if (hedge->terminationRequired) + { + break; + } + + if (hedgeReceiveCheck()) + {// hedgehog data received + ROS_INFO("%d, %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.timestamp_ms, + (int) (hedge_pos_msg.timestamp_ms - hedge_timestamp_prev), + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (int) hedge_pos_msg.flags); + hedge_pos_publisher.publish(hedge_pos_msg); + + hedge_timestamp_prev= hedge_pos_msg.timestamp_ms; + } + + ros::spinOnce(); + + loop_rate.sleep(); + } + + // Exit + if (hedge != NULL) + { + stopMarvelmindHedge (hedge); + destroyMarvelmindHedge (hedge); + } + + return 0; +} diff --git a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c new file mode 100644 index 00000000..205f2117 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c @@ -0,0 +1,496 @@ +#include +#include +#include +#ifdef WIN32 +#include +#include +#else +#include +#include +#include +#include +#endif // WIN32 +#include "marvelmind_nav/marvelmind_hedge.h" + +uint8_t positionDatagramHeader[]=POSITION_DATAGRAM_HEADER; + +////////////////////////////////////////////////////////////////////////////// +// Calculate CRC (Modbus) for array of bytes +// buf: input buffer +// len: size of buffer +// returncode: CRC value +////////////////////////////////////////////////////////////////////////////// +uint16_t CalcCrcModbus_(uint8_t * buf, int len) +{ + uint16_t crc = 0xFFFF; + int pos; + for (pos = 0; pos < len; pos++) + { + crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc + int i; + for (i = 8; i != 0; i--) // Loop over each bit + { + if ((crc & 0x0001) != 0) // If the LSB is set + { + crc >>= 1; // Shift right and XOR 0xA001 + crc ^= 0xA001; + } + else // Else LSB is not set + crc >>= 1; // Just shift right + } + } + return crc; +} + +#ifdef WIN32 +#define SERIAL_PORT_HANDLE HANDLE +#define PORT_NOT_OPENED INVALID_HANDLE_VALUE +////////////////////////////////////////////////////////////////////////////// +// Open Serial Port (Windows only) +// portFileName: alias of port (e.g. "COM3"). Add prefix "\\\\.\\" before alias +// to open higher ports (e.g. COM12) +// baudrate: baudRate rate (e.g. 19200) +// verbose: print errors +// returncode: valid handle if port is successfully opened or +// INVALID_HANDLE_VALUE on error +////////////////////////////////////////////////////////////////////////////// +HANDLE OpenSerialPort_ (const char * portFileName, uint32_t baudrate, + bool verbose) +{ + HANDLE ttyHandle = CreateFile( TEXT(portFileName), GENERIC_READ, 0, + NULL, OPEN_EXISTING, 0, NULL); + if (ttyHandle==INVALID_HANDLE_VALUE) + { + if (verbose) + { + puts(portFileName); + puts ("Error: unable to open serial connection " + "(possibly serial port is not available)"); + } + return INVALID_HANDLE_VALUE; + } + COMMTIMEOUTS timeouts= {3000,3000,3000,3000,3000}; + bool returnCode=SetCommTimeouts (ttyHandle, &timeouts); + if (!returnCode) + { + if (verbose) puts ("Error: unable to set serial port timeouts"); + CloseHandle (ttyHandle); + return INVALID_HANDLE_VALUE; + } + DCB dcb= {0}; + returnCode=GetCommState (ttyHandle, &dcb); + if (!returnCode) + { + if (verbose) puts ("Error: unable to get serial port parameters"); + CloseHandle (ttyHandle); + return INVALID_HANDLE_VALUE; + } + dcb.BaudRate = baudrate; + dcb.fAbortOnError=true; + returnCode=SetCommState (ttyHandle, &dcb); + if (!returnCode) + { + if (verbose) puts ("Error: unable to set serial port parameters"); + CloseHandle (ttyHandle); + return INVALID_HANDLE_VALUE; + } + return ttyHandle; +} +#else +////////////////////////////////////////////////////////////////////////////// +// Converts baudrate value to baudRate code (Linux only) +// baudrate: value of baudRate rate (e.g. 19200) +// verbose: show errors +// returncode: code of baudRate rate (e.g. B19200) +////////////////////////////////////////////////////////////////////////////// +uint32_t _GetBaudCode (uint32_t baudrate, bool verbose) +{ + switch (baudrate) + { + case 50: + return B50; + case 75: + return B75; + case 110: + return B110; + case 134: + return B134; + case 150: + return B150; + case 200: + return B200; + case 300: + return B300; + case 600: + return B600; + case 1200: + return B1200; + case 1800: + return B1800; + case 2400: + return B2400; + case 4800: + return B4800; + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + default: + if (verbose) + printf ("Warning: unsupported baudrate %u. Using 9600.\n", + baudrate); + return B9600; + } +} + +#define SERIAL_PORT_HANDLE int +#define PORT_NOT_OPENED -1 +////////////////////////////////////////////////////////////////////////////// +// Open Serial Port (Linux only) +// portFileName: alias of port (e.g. "/dev/ttyACM0") +// baudrate: baudRate rate (e.g. 19200) +// verbose: show errors +// returncode: valid handle if port is successfully opened or -1 on error +////////////////////////////////////////////////////////////////////////////// +int OpenSerialPort_ (const char * portFileName, uint32_t baudrate, bool verbose) +{ + int ttyHandle = open(portFileName, O_RDWR| O_NONBLOCK | O_NDELAY ); + if (ttyHandle<0) + { + if (verbose) + { + puts(portFileName); + puts ("Error: unable to open serial connection " + "(possibly serial port is not available)"); + } + return -1; + } + struct termios ttyCtrl; + memset (&ttyCtrl, 0, sizeof ttyCtrl); + if ( tcgetattr ( ttyHandle, &ttyCtrl ) != 0 ) + { + if (verbose) puts ("Error: unable to get serial port parameters"); + return -1; + } + + uint32_t baudCode=_GetBaudCode(baudrate, verbose); + cfsetospeed (&ttyCtrl, baudCode); + cfsetispeed (&ttyCtrl, baudCode); + // 8N1, no flow control + ttyCtrl.c_cflag &= ~(PARENB|CSTOPB|CSIZE|CRTSCTS); + ttyCtrl.c_cflag |= CS8; + // no signaling chars, no echo, no canonical processing + ttyCtrl.c_lflag = 0; + ttyCtrl.c_oflag = 0; // no remapping, no delays + ttyCtrl.c_cc[VMIN] = 0; // read doesn't block + ttyCtrl.c_cc[VTIME] = 30; // 3 seconds read timeout + ttyCtrl.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines + ttyCtrl.c_iflag &= ~(IXON | IXOFF | IXANY);// turn off s/w flow ctrl + ttyCtrl.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw + ttyCtrl.c_oflag &= ~OPOST; // make raw + tcflush(ttyHandle, TCIFLUSH ); // Flush port + if (tcsetattr (ttyHandle, TCSANOW, &ttyCtrl) != 0) + { + if (verbose) puts ("Error: unable to set serial port parameters"); + return -1; + } + return ttyHandle; +} +#endif + + + +enum +{ + RECV_HDR, + RECV_DGRAM +}; + +////////////////////////////////////////////////////////////////////////////// +// Thread function started by MarvelmindHedge_start +////////////////////////////////////////////////////////////////////////////// +void +#ifndef WIN32 +* +#endif // WIN32 +Marvelmind_Thread_ (void* param) +{ + struct MarvelmindHedge * hedge=(struct MarvelmindHedge*) param; + struct PositionDatagram_ positionDatagram; + uint8_t recvState=RECV_HDR; // current state of receive data + uint8_t nBytesInBlockReceived=0; // bytes received + + uint8_t lastValues_next=0; + + SERIAL_PORT_HANDLE ttyHandle=OpenSerialPort_(hedge->ttyFileName, + hedge->baudRate, hedge->verbose); + if (ttyHandle==PORT_NOT_OPENED) hedge->terminationRequired=true; + else if (hedge->verbose) printf ("Opened serial port %s with baudrate %u\n", + hedge->ttyFileName, hedge->baudRate); + + while (hedge->terminationRequired==false) + { + uint8_t receivedChar; + bool readSuccessed=true; +#ifdef WIN32 + uint32_t nBytesRead; + readSuccessed=ReadFile (ttyHandle, &receivedChar, 1, &nBytesRead, NULL); +#else + int32_t nBytesRead; + nBytesRead=read(ttyHandle, &receivedChar, 1); + if (nBytesRead<0) readSuccessed=false; +#endif + if (nBytesRead && readSuccessed) + { + //printf("%x %d %d ", receivedChar & 0xff, nBytesRead, readSuccessed); + + *((char*)&positionDatagram+nBytesInBlockReceived)=receivedChar; + switch (recvState) + { + case RECV_HDR: + if (receivedChar==positionDatagramHeader[nBytesInBlockReceived]) + { + // correct header byte + nBytesInBlockReceived++; + if (nBytesInBlockReceived>=sizeof(positionDatagramHeader)) + { + recvState=RECV_DGRAM; + } + } + else + { + // ...or incorrect + nBytesInBlockReceived=0; + } + break; + case RECV_DGRAM: + nBytesInBlockReceived++; + if (nBytesInBlockReceived>=sizeof(struct PositionDatagram_)) + { + // parse dgram + uint16_t blockCrc= + CalcCrcModbus_((uint8_t*)&positionDatagram, + sizeof(struct PositionDatagram_)-2); + if (blockCrc==positionDatagram.crc) + { + // add to positionBuffer +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + hedge->positionBuffer[lastValues_next].timestamp= + positionDatagram.timestamp; + hedge->positionBuffer[lastValues_next].x= + positionDatagram.x; + hedge->positionBuffer[lastValues_next].y= + positionDatagram.y; + hedge->positionBuffer[lastValues_next].z= + positionDatagram.z; + hedge->positionBuffer[lastValues_next].flags= + positionDatagram.flags; + lastValues_next++; + if (lastValues_next>=hedge->maxBufferedPositions) + lastValues_next=0; + if (hedge->lastValuesCount_maxBufferedPositions) + hedge->lastValuesCount_++; + hedge->haveNewValues_=true; +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + // callback + if (hedge->receiveDataCallback) + { + struct PositionValue position= + { + positionDatagram.timestamp, + positionDatagram.x, + positionDatagram.y, + positionDatagram.z, + positionDatagram.flags + }; + hedge->receiveDataCallback (position); + } + } + // and repeat + recvState=RECV_HDR; + nBytesInBlockReceived=0; + } + } + } + } +#ifndef WIN32 + return NULL; +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Create an initialize MarvelmindHedge structure +// returncode: pointer to structure on success or NULL on error +////////////////////////////////////////////////////////////////////////////// +struct MarvelmindHedge * createMarvelmindHedge () +{ + struct MarvelmindHedge * hedge=malloc (sizeof (struct MarvelmindHedge)); + if (hedge) + { + hedge->ttyFileName=DEFAULT_TTY_FILENAME; + hedge->baudRate=9600; + hedge->maxBufferedPositions=1; + hedge->positionBuffer=NULL; + hedge->verbose=false; + hedge->receiveDataCallback=NULL; + hedge->lastValuesCount_=0; + hedge->haveNewValues_=false; + hedge->terminationRequired= false; +#ifdef WIN32 + InitializeCriticalSection(&hedge->lock_); +#else + pthread_mutex_init (&hedge->lock_, NULL); +#endif + } + else puts ("Not enough memory"); + return hedge; +} + +////////////////////////////////////////////////////////////////////////////// +// Initialize and start work thread +////////////////////////////////////////////////////////////////////////////// +void startMarvelmindHedge (struct MarvelmindHedge * hedge) +{ + hedge->positionBuffer= + malloc(sizeof (struct PositionValue)*hedge->maxBufferedPositions); + if (hedge->positionBuffer==NULL) + { + if (hedge->verbose) puts ("Not enough memory"); + hedge->terminationRequired=true; + return; + } +#ifdef WIN32 + _beginthread (Marvelmind_Thread_, 0, hedge); +#else + pthread_create (&hedge->thread_, NULL, Marvelmind_Thread_, hedge); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Write average position coordinates +// hedge: MarvelmindHedge structure +// position: pointer to PositionValue for write coordinates +// returncode: true if position is valid +////////////////////////////////////////////////////////////////////////////// +bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct PositionValue * position) +{ + uint8_t i; + int16_t avg_x=0, avg_y=0, avg_z=0; + uint32_t max_timestamp=0; + uint8_t flags= 0; + bool position_valid; +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + if (hedge->lastValuesCount_) + { + uint8_t real_values_count=hedge->maxBufferedPositions; + if (hedge->lastValuesCount_lastValuesCount_; + position_valid=true; + for (i=0; ipositionBuffer[i].x; + avg_y+=hedge->positionBuffer[i].y; + avg_z+=hedge->positionBuffer[i].z; + if (hedge->positionBuffer[i].timestamp>max_timestamp) + max_timestamp=hedge->positionBuffer[i].timestamp; + + flags= hedge->positionBuffer[i].flags; + if (flags&(1<<0)) + { + position_valid=false; + } + } + avg_x/=real_values_count; + avg_y/=real_values_count; + avg_z/=real_values_count; + + if (!position_valid) + flags|= (1<<0);// coordiantes not available + } + else position_valid=false; +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + position->x=avg_x; + position->y=avg_y; + position->z=avg_z; + position->timestamp=max_timestamp; + position->flags= flags; + return position_valid; +} + +////////////////////////////////////////////////////////////////////////////// +// Print average position coordinates +// onlyNew: print only new positions +////////////////////////////////////////////////////////////////////////////// +void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew) +{ + if (hedge->haveNewValues_ || (!onlyNew)) + { + struct PositionValue position; + getPositionFromMarvelmindHedge (hedge, &position); + printf ("X: %d, Y: %d, Z: %d at time T: %u\n", position.x, + position.y, position.z, position.timestamp); + hedge->haveNewValues_=false; + } +} + +////////////////////////////////////////////////////////////////////////////// +// Stop work thread +////////////////////////////////////////////////////////////////////////////// +void stopMarvelmindHedge (struct MarvelmindHedge * hedge) +{ + hedge->terminationRequired=true; + if (hedge->verbose) puts ("stopping"); +#ifdef WIN32 + WaitForSingleObject (hedge->thread_, INFINITE); +#else + pthread_join (hedge->thread_, NULL); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Destroy structures to free memory (You must call stopMarvelmindHedge +// first) +////////////////////////////////////////////////////////////////////////////// +void destroyMarvelmindHedge (struct MarvelmindHedge * hedge) +{ + if (hedge->positionBuffer) free (hedge->positionBuffer); + free (hedge); +} diff --git a/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp b/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp new file mode 100644 index 00000000..b03d4ac8 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp @@ -0,0 +1,95 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "marvelmind_nav/hedge_pos.h" +#include + +#define ROS_NODE_NAME "subscriber_test" +#define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" + +#define SCALE_HEDGE 3.0 + +ros::Publisher rviz_marker_pub; +uint32_t rviz_shape; + + +void showRvizObject(float x, float y, float z) +{ + if (rviz_marker_pub.getNumSubscribers() < 1) return; + + visualization_msgs::Marker marker; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + marker.header.frame_id = "/my_frame"; + marker.header.stamp = ros::Time::now(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + marker.ns = "basic_shapes"; + marker.id = 0; + + // Set the marker type + marker.type = rviz_shape; + + // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) + marker.action = visualization_msgs::Marker::ADD; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + marker.pose.position.x = x; + marker.pose.position.y = y; + marker.pose.position.z = z; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + marker.scale.x = 0.05*SCALE_HEDGE; + marker.scale.y = 0.05*SCALE_HEDGE; + marker.scale.z = 0.02*SCALE_HEDGE; + // Set the color -- be sure to set alpha to something non-zero! + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0; + + marker.lifetime = ros::Duration(5); + + rviz_marker_pub.publish(marker); +} + +void hedgePosCallback(const marvelmind_nav::hedge_pos& hedge_pos_msg) +{ + ROS_INFO("Hedgehog data: %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.timestamp_ms, + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (int) hedge_pos_msg.flags); + + if ((hedge_pos_msg.flags&(1<<0))==0) + { + showRvizObject(hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m); + } +} + +/** + * Test subscriber node for getting data from Marvelmind publishers nodes + */ +int main(int argc, char **argv) +{ + + // initialize ROS node + ros::init(argc, argv, ROS_NODE_NAME); + + // ROS node reference + ros::NodeHandle n; + + // Declare need to subscribe data from topic + ros::Subscriber sub = n.subscribe(HEDGE_POSITION_TOPIC_NAME, 1000, hedgePosCallback); + + // Declare publisher for rviz visualization + rviz_marker_pub = n.advertise("visualization_marker", 1); + // Set our initial shape type to be a cube + rviz_shape = visualization_msgs::Marker::CUBE; + + ros::spin(); + + return 0; +} From a375478ea2e34026ab47b3db7370302af6bece53 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 14 Oct 2016 19:21:54 -0700 Subject: [PATCH 065/183] Optimized julia codes for speed, cleaned code. --- eval_sim/eval_run.jl | 46 ++++++----- .../src/barc/launch/closedLoop_mpc.launch | 38 --------- .../launch/estimation_KinBkMdl_gps.launch | 42 ---------- .../src/barc/launch/run_lmpc_barc.launch | 49 ++++++++++++ ...ote_lmpc.launch => run_lmpc_remote.launch} | 0 workspace/src/barc/src/LMPC_node.jl | 8 +- workspace/src/barc/src/barc_record.jl | 78 ++++++++----------- workspace/src/barc/src/barc_simulator_dyn.jl | 74 +++++++++--------- 8 files changed, 149 insertions(+), 186 deletions(-) delete mode 100644 workspace/src/barc/launch/closedLoop_mpc.launch delete mode 100755 workspace/src/barc/launch/estimation_KinBkMdl_gps.launch create mode 100644 workspace/src/barc/launch/run_lmpc_barc.launch rename workspace/src/barc/launch/{run_remote_lmpc.launch => run_lmpc_remote.launch} (100%) diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl index 3beba75d..1431694c 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_run.jl @@ -1,6 +1,17 @@ using JLD using PyPlot using HDF5, JLD, ProfileView +# pos_info[1] = s +# pos_info[2] = eY +# pos_info[3] = ePsi +# pos_info[4] = v +# pos_info[5] = s_start +# pos_info[6] = x +# pos_info[7] = y +# pos_info[8] = v_x +# pos_info[9] = v_y +# pos_info[10] = psi +# pos_info[11] = psiDot include("../workspace/src/barc/src/LMPC_lib/classes.jl") @@ -11,7 +22,7 @@ type Measurements{T} end -const log_path = "$(homedir())/simulations/record-2016-10-13-20-03-58.jld" +#const log_path = "$(homedir())/simulations/record-2016-10-13-20-03-58.jld" const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" @@ -32,7 +43,7 @@ function simModel(z,u,dt,l_A,l_B) return zNext end -function eval_run() +function eval_run(log_path) d = load(log_path) imu_meas = d["imu_meas"] @@ -45,7 +56,7 @@ function eval_run() track = create_track(0.3) figure() - plot(gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") + plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid(1) title("x-y-view") @@ -53,44 +64,41 @@ function eval_run() legend(["GPS meas","estimate"]) figure() - plot(gps_meas.t,gps_meas.z,"-*",pos_info.t,pos_info.z[:,6:7],"-o") + title("x/y measurements and estimate") + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x") grid(1) title("GPS comparison") legend(["x_meas","y_meas","x_est","y_est"]) figure() title("Comparison of psi") - plot(imu_meas.t,imu_meas.z[:,6],imu_meas.t,imu_meas.z[:,3],"-x",pos_info.t,pos_info.z[:,10:11],"-*") + plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*") legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot"]) grid() figure() title("Raw IMU orientation data") - plot(imu_meas.t,imu_meas.z[:,1:3],"--",imu_meas.t,imu_meas.z[:,4:6]) + plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6]) grid("on") legend(["w_x","w_y","w_z","roll","pitch","yaw"]) figure() - title("Comparison of v") - plot(pos_info.t,pos_info.z[:,8:9],"-*",vel_est.t,vel_est.z) + title("v measurements and estimate") + plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z) legend(["est_xDot","est_yDot","v_raw"]) grid() figure() - subplot(211) - plot(pos_info.t,pos_info.z[:,6:7]) + title("s, eY and inputs") + ax1=subplot(211) + plot(pos_info.t-t0,pos_info.z[:,2:3],"-*") grid("on") - legend(["x","y"]) - subplot(212) - plot(cmd_log.t,cmd_log.z) + legend(["eY","ePsi"]) + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-*") grid("on") legend(["u","d_f"]) - - - figure() - plot(cmd_log.t-t0,cmd_log.z) - legend(["a","d_f"]) - grid() + nothing end function eval_LMPC() diff --git a/workspace/src/barc/launch/closedLoop_mpc.launch b/workspace/src/barc/launch/closedLoop_mpc.launch deleted file mode 100644 index 8298e38a..00000000 --- a/workspace/src/barc/launch/closedLoop_mpc.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/estimation_KinBkMdl_gps.launch b/workspace/src/barc/launch/estimation_KinBkMdl_gps.launch deleted file mode 100755 index d4b92421..00000000 --- a/workspace/src/barc/launch/estimation_KinBkMdl_gps.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/run_lmpc_barc.launch b/workspace/src/barc/launch/run_lmpc_barc.launch new file mode 100644 index 00000000..fe838995 --- /dev/null +++ b/workspace/src/barc/launch/run_lmpc_barc.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/run_remote_lmpc.launch b/workspace/src/barc/launch/run_lmpc_remote.launch similarity index 100% rename from workspace/src/barc/launch/run_remote_lmpc.launch rename to workspace/src/barc/launch/run_lmpc_remote.launch diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index b8e962d5..1bf25c4e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -86,9 +86,9 @@ function main() # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) - pub = Publisher("ecu", ECU, queue_size=1) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1) + s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} println("Finished initialization.") # Lap parameters @@ -126,8 +126,8 @@ function main() # ============================= PUBLISH COMMANDS ============================= # this is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received # the state is predicted by 0.1s - cmd.motor = mpcSol.a_x - cmd.servo = mpcSol.d_f + cmd.motor = convert(Float32,mpcSol.a_x) + cmd.servo = convert(Float32,mpcSol.d_f) publish(pub, cmd) # ============================= Initialize iteration parameters ============================= diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index c259e35c..c4e2b467 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -1,17 +1,13 @@ #!/usr/bin/env julia #= - Licensing Information: You are free to use or extend these projects for - education or reserach purposes provided that (1) you retain this notice - and (2) you provide clear attribution to UC Berkeley, including a link - to http://barc-project.com - - Attibution Information: The barc project ROS code-base was developed - at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales - (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed - by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was - based on an open source project by Bruce Wootton -=# +This node listens to and records messages of following topics: +Raw GPS (/hedge_pos) +Raw IMU (/imu/data) +Commands (/ecu) +Raw velocity estimation (/vel_est) +Position info (/pos_info) +=# using RobotOS @rosimport barc.msg: ECU, pos_info @@ -42,19 +38,6 @@ function clean_up(m::Measurements) m.z = m.z[1:m.i-1,:] end -buffersize = 60000 -gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,9)) -cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -vel_est_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) -pos_info_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,11)) - -gps_meas.t[1] = time() -imu_meas.t[1] = time() -cmd_log.t[1] = time() -pos_info_log.t[1] = time() -vel_est_log.t[1] = time() - function Quat2Euler(q::Array{Float64}) sol = zeros(Float64,3) sol[1] = atan2(2*(q[1]*q[2]+q[3]*q[4]),1-2*(q[2]^2+q[3]^2)) @@ -63,17 +46,14 @@ function Quat2Euler(q::Array{Float64}) return sol end -function ECU_callback(msg::ECU) - global cmd_log - u_current = convert(Array{Float64,1},[msg.motor, msg.servo]) +function ECU_callback(msg::ECU,cmd_log::Measurements) cmd_log.i += 1 cmd_log.t[cmd_log.i] = time() - cmd_log.z[cmd_log.i,:] = u_current + cmd_log.z[cmd_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) nothing end -function IMU_callback(msg::Imu) - global imu_meas +function IMU_callback(msg::Imu,imu_meas::Measurements) imu_meas.i += 1 imu_meas.t[imu_meas.i] = time() imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z; @@ -82,46 +62,54 @@ function IMU_callback(msg::Imu) nothing end -function GPS_callback(msg::hedge_pos) - global gps_meas +function GPS_callback(msg::hedge_pos,gps_meas::Measurements) gps_meas.i += 1 gps_meas.t[gps_meas.i] = time() gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] nothing end -function pos_info_callback(msg::pos_info) - global pos_info_log +function pos_info_callback(msg::pos_info,pos_info_log::Measurements) pos_info_log.i += 1 pos_info_log.t[pos_info_log.i] = time() pos_info_log.z[pos_info_log.i,:] = [msg.s;msg.ey;msg.epsi;msg.v;msg.s_start;msg.x;msg.y;msg.v_x;msg.v_y;msg.psi;msg.psiDot] nothing end -function vel_est_callback(msg::Float32Msg) - global vel_est_log +function vel_est_callback(msg::Float32Msg,vel_est_log::Measurements) vel_est_log.i += 1 vel_est_log.t[vel_est_log.i] = time() - vel_est_log.z[vel_est_log.i] = msg.data + vel_est_log.z[vel_est_log.i] = convert(Float64,msg.data) nothing end function main() + + buffersize = 60000 + gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,9)) + cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + vel_est_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) + pos_info_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,11)) + + gps_meas.t[1] = time() + imu_meas.t[1] = time() + cmd_log.t[1] = time() + pos_info_log.t[1] = time() + vel_est_log.t[1] = time() # initiate node, set up publisher / subscriber topics init_node("barc_record") - s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) - s2 = Subscriber("imu/data", Imu, IMU_callback, queue_size=1) - s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, queue_size=1) - s5 = Subscriber("pos_info", pos_info, pos_info_callback, queue_size=1) - s6 = Subscriber("vel_est", Float32Msg, vel_est_callback, queue_size=1) + s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} + s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} + s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s6 = Subscriber("vel_est", Float32Msg, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{std_msgs.msg.Float32Msg} dt = 0.1 loop_rate = Rate(1/dt) println("Recorder running.") - while ! is_shutdown() - rossleep(loop_rate) - end + spin() # wait for callbacks until shutdown # Clean up buffers clean_up(gps_meas) diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index a36ee249..968b9072 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -32,10 +32,6 @@ using JLD include("LMPC_lib/classes.jl") include("LMPC_lib/simModel.jl") -u_current = zeros(Float64,2) # msg ECU is Float32 ! - -t = 0 - # This type contains measurement data (time, values and a counter) type Measurements{T} i::Int64 # measurement counter @@ -49,35 +45,36 @@ function clean_up(m::Measurements) m.z = m.z[1:m.i-1,:] end -buffersize = 60000 -gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,8)) -slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - -z_real.t[1] = time() -slip_a.t[1] = time() -imu_meas.t[1] = time() -cmd_log.t[1] = time() - -function ECU_callback(msg::ECU) - global u_current - u_current = convert(Array{Float64,1},[msg.motor, msg.servo]) +function ECU_callback(msg::ECU,u_current::Array{Float64},cmd_log::Measurements) + u_current[:] = convert(Array{Float64,1},[msg.motor, msg.servo]) cmd_log.i += 1 cmd_log.t[cmd_log.i] = time() cmd_log.z[cmd_log.i,:] = u_current end function main() + u_current = zeros(Float64,2) # msg ECU is Float32 ! + + buffersize = 60000 + gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,8)) + slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) + + z_real.t[1] = time() + slip_a.t[1] = time() + imu_meas.t[1] = time() + cmd_log.t[1] = time() + # initiate node, set up publisher / subscriber topics init_node("barc_sim") - pub_enc = Publisher("encoder", Encoder, queue_size=1) - pub_gps = Publisher("hedge_pos", hedge_pos, queue_size=1) - pub_imu = Publisher("imu/data", Imu, queue_size=1) - pub_vel = Publisher("vel_est", Float32Msg, queue_size=1) + #pub_enc = Publisher("encoder", Encoder, queue_size=1)::RobotOS.Publisher{barc.msg.Encoder} + pub_gps = Publisher("hedge_pos", hedge_pos, queue_size=1)::RobotOS.Publisher{marvelmind_nav.msg.hedge_pos} + pub_imu = Publisher("imu/data", Imu, queue_size=1)::RobotOS.Publisher{sensor_msgs.msg.Imu} + pub_vel = Publisher("vel_est", Float32Msg, queue_size=1)::RobotOS.Publisher{std_msgs.msg.Float32Msg} - s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=1) + s1 = Subscriber("ecu", ECU, ECU_callback, (u_current,cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} z_current = zeros(60000,8) z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0 0.0] @@ -88,8 +85,8 @@ function main() i = 2 - dist_traveled = 0 - last_updated = 0 + dist_traveled = 0.0 + last_updated = 0.0 r_tire = 0.036 # radius from tire center to perimeter along magnets [m] quarterCirc = 0.5 * pi * r_tire # length of a quarter of a tire, distance from one to the next encoder @@ -114,8 +111,9 @@ function main() println("Publishing sensor information. Simulator running.") imu_data = Imu() - vel_est = Float32Msg() - t0 = time() + vel_est = Float32Msg() + t0 = time() + t = 0.0 while ! is_shutdown() t = time() @@ -129,16 +127,16 @@ function main() slip_a.t[i] = t # Encoder measurements calculation - dist_traveled += z_current[i,3]*dt #count the total traveled distance since the beginning of the simulation - if dist_traveled - last_updated >= quarterCirc - last_updated = dist_traveled - FL += 1 - FR += 1 - BL += 1 - BR += 0 #no encoder on back right wheel - enc_data = Encoder(FL, FR, BL, BR) - publish(pub_enc, enc_data) #publish a message everytime the encoder counts up - end + # dist_traveled += z_current[i,3]*dt #count the total traveled distance since the beginning of the simulation + # if dist_traveled - last_updated >= quarterCirc + # last_updated = dist_traveled + # FL += 1 + # FR += 1 + # BL += 1 + # BR += 0 #no encoder on back right wheel + # enc_data = Encoder(FL, FR, BL, BR) + # publish(pub_enc, enc_data) #publish a message everytime the encoder counts up + # end # IMU measurements imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) From 975f2048a3f5c2c44ae2684631f3df4889abe59e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Sun, 16 Oct 2016 14:42:39 -0700 Subject: [PATCH 066/183] Improved simulation model (added artificial sensor disturbances) --- .../arduino_nano328_node.ino | 36 +--------- eval_sim/eval_run.jl | 21 +++++- .../src/barc/launch/run_lmpc_barc.launch | 6 +- .../src/barc/launch/run_lmpc_remote.launch | 10 ++- .../src/barc/launch/sensor_test_barc.launch | 4 +- .../src/barc/launch/sensor_test_remote.launch | 6 +- workspace/src/barc/src/barc_simulator_dyn.jl | 71 ++++++++++--------- 7 files changed, 66 insertions(+), 88 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index df250666..ccc04864 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -262,44 +262,10 @@ void loop() { if (dt > 50) { car.readAndCopyInputs(); - // TODO make encoder and rc_inputs private properties on car? Then - // car.publishEncoder(); and car.publishRCInputs(); - //encoder.FL = car.getEncoderFL(); - //encoder.FR = car.getEncoderFR(); - //encoder.BL = car.getEncoderBL(); - //encoder.BR = car.getEncoderBR(); - // pub_encoder.publish(&encoder); - - // pubblish encoder dt - //encoder_dt_FL.data = (int32_t) car.getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) - //encoder_dt_FR.data = (int32_t) car.getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) - //encoder_dt_BL.data = (int32_t) car.getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) - //encoder_dt_BR.data = (int32_t) car.getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) - vel_est.data = car.getVelocityEstimate(); - //pub_encoder_dt_FL.publish(&encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) - //pub_encoder_dt_FR.publish(&encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) - //pub_encoder_dt_BL.publish(&encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) - //pub_encoder_dt_BR.publish(&encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) - + vel_est.data = car.getVelocityEstimate() pub_vel_est.publish(&vel_est); // publish estimated velocity ////////////////////////////////////////////////!!!! - - - - - //rc_inputs.motor = car.getRCThrottle(); - //rc_inputs.servo = car.getRCSteering(); - //pub_rc_inputs.publish(&rc_inputs); - - // publish ultra-sound measurement - /* - ultrasound.front = us_fr.getRange(); - ultrasound.back = us_bk.getRange(); - ultrasound.right = us_rt.getRange(); - ultrasound.left = us_lt.getRange(); - pub_ultrasound.publish(&ultrasound); - */ t0 = millis(); } diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_run.jl index 1431694c..e2637741 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_run.jl @@ -26,6 +26,23 @@ end const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" +function initPlot() + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + function simModel(z,u,dt,l_A,l_B) # kinematic bicycle model @@ -68,6 +85,8 @@ function eval_run(log_path) plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x") grid(1) title("GPS comparison") + xlabel("t [s]") + ylabel("Position [m]") legend(["x_meas","y_meas","x_est","y_est"]) figure() @@ -84,7 +103,7 @@ function eval_run(log_path) figure() title("v measurements and estimate") - plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z) + plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z,"-x") legend(["est_xDot","est_yDot","v_raw"]) grid() diff --git a/workspace/src/barc/launch/run_lmpc_barc.launch b/workspace/src/barc/launch/run_lmpc_barc.launch index fe838995..fa0bdddd 100644 --- a/workspace/src/barc/launch/run_lmpc_barc.launch +++ b/workspace/src/barc/launch/run_lmpc_barc.launch @@ -36,10 +36,8 @@ - - - - + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index db63a668..cdbee9e5 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -22,8 +22,8 @@ - - + + @@ -42,10 +42,8 @@ - - - - + + diff --git a/workspace/src/barc/launch/sensor_test_barc.launch b/workspace/src/barc/launch/sensor_test_barc.launch index eddb31fb..23a93a2e 100644 --- a/workspace/src/barc/launch/sensor_test_barc.launch +++ b/workspace/src/barc/launch/sensor_test_barc.launch @@ -29,9 +29,7 @@ - - - + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index 7682d1e8..6349fd58 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -34,10 +34,8 @@ - - - - + + diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 968b9072..0ee7230c 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -114,60 +114,61 @@ function main() vel_est = Float32Msg() t0 = time() t = 0.0 + + sim_gps_interrupt = 0 # counter if gps interruption is simulated + vel_pos = zeros(2) # position when velocity was updated last time + vel_dist_update = 2*pi*0.036/2 # distance to travel until velocity is updated (half wheel rotation) while ! is_shutdown() t = time() # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:],u_current', dt, modelParams) - #println("z_current:") - #println(z_current[i,:]) - #println(slip_ang[i,:]) - z_real.t[i] = t slip_a.t[i] = t - # Encoder measurements calculation - # dist_traveled += z_current[i,3]*dt #count the total traveled distance since the beginning of the simulation - # if dist_traveled - last_updated >= quarterCirc - # last_updated = dist_traveled - # FL += 1 - # FR += 1 - # BL += 1 - # BR += 0 #no encoder on back right wheel - # enc_data = Encoder(FL, FR, BL, BR) - # publish(pub_enc, enc_data) #publish a message everytime the encoder counts up - # end - # IMU measurements - imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) - yaw = z_current[i,5] + randn()*0.05 + imu_drift - psiDot = z_current[i,6] + 0.01*randn() - imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) - imu_data.angular_velocity = Vector3(0,0,psiDot) - - # Velocity measurement - vel_est.data = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) - if i%2 == 0 + if i%2 == 0 # 50 Hz + imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) + yaw = z_current[i,5] + randn()*0.02 + imu_drift + psiDot = z_current[i,6] + 0.01*randn() imu_meas.i += 1 imu_meas.t[imu_meas.i] = t imu_meas.z[imu_meas.i,:] = [yaw psiDot] + imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) + imu_data.angular_velocity = Vector3(0,0,psiDot) publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) - publish(pub_vel, vel_est) + end + # Velocity measurements + if i%5 == 0 # 20 Hz + if norm(z_current[i,1:2][:]-vel_pos) > vel_dist_update # only update if a magnet has passed the sensor + vel_est.data = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) + vel_pos = z_current[i,1:2][:] + end + publish(pub_vel, vel_est) end # GPS measurements - x = round(z_current[i,1] + 0.01*randn()*2,2) # Indoor gps measures in cm - y = round(z_current[i,2] + 0.01*randn()*2,2) - if i % 3 == 0 - gps_meas.i += 1 - gps_meas.t[gps_meas.i] = t - gps_meas.z[gps_meas.i,:] = [x y] - gps_data = hedge_pos(0,x,y,0,0) - publish(pub_gps, gps_data) + if i % 4 == 0 # 25 Hz + x = round(z_current[i,1] + 0.02*randn(),2) # Indoor gps measures, rounded on cm + y = round(z_current[i,2] + 0.02*randn(),2) + if randn()>3 # simulate gps-outlier (probability about 0.13% for randn()>3, ) + x += randn() # add random value to x and y + y += randn() + sim_gps_interrupt = 4 # also do a little interruption + elseif randn()>3 && sim_gps_interrupt < 0 + sim_gps_interrupt = 10 # simulate gps-interrupt (10 steps at 25 Hz is 0.4 seconds) + end + if sim_gps_interrupt < 0 + gps_meas.i += 1 + gps_meas.t[gps_meas.i] = t + gps_meas.z[gps_meas.i,:] = [x y] + gps_data = hedge_pos(0,x,y,0,0) + publish(pub_gps, gps_data) + end + sim_gps_interrupt -= 1 end - i += 1 rossleep(loop_rate) end From 93728b7c2abd795a7c87103399c50c92c655e026 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 18 Oct 2016 18:27:14 -0700 Subject: [PATCH 067/183] Introduced extrapolation of GPS and psiDot values, added outlier detection for GPS. Combined evaluation in one single file for simulation and experiment evaluation. Changed messages so that they include timestamp. --- .../arduino_nano328_node.ino | 7 +- eval_sim/{eval_run.jl => eval_data.jl} | 178 +++++-- eval_sim/eval_sim.jl | 438 ------------------ workspace/src/barc/CMakeLists.txt | 1 + workspace/src/barc/launch/barc_sim.launch | 2 +- .../src/barc/launch/run_lmpc_barc.launch | 4 +- workspace/src/barc/msg/Vel_est.msg | 2 + workspace/src/barc/src/LMPC_node.jl | 2 +- workspace/src/barc/src/barc_record.jl | 8 +- workspace/src/barc/src/barc_simulator_dyn.jl | 22 +- .../src/barc/src/state_estimation_DynBkMdl.py | 121 ++++- .../ros_marvelmind_package/msg/hedge_pos.msg | 1 + .../src/hedge_rcv_bin.cpp | 5 +- .../src/marvelmind_hedge.c | 2 +- 14 files changed, 266 insertions(+), 527 deletions(-) rename eval_sim/{eval_run.jl => eval_data.jl} (75%) delete mode 100644 eval_sim/eval_sim.jl create mode 100644 workspace/src/barc/msg/Vel_est.msg diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index ccc04864..915b1933 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -22,6 +22,7 @@ WARNING: // include libraries #include //#include +#include #include #include #include @@ -197,7 +198,8 @@ barc::ECU ecu; //std_msgs::Int32 encoder_dt_FR; //(ADDED BY TOMMI 7JULY2016) //std_msgs::Int32 encoder_dt_BL; //(ADDED BY TOMMI 7JULY2016) //std_msgs::Int32 encoder_dt_BR; //(ADDED BY TOMMI 7JULY2016) -std_msgs::Float32 vel_est; // estimation of current velocity to be published +//std_msgs::Float32 vel_est; // estimation of current velocity to be published +barc::Vel_est vel_est; ros::NodeHandle nh; @@ -262,7 +264,8 @@ void loop() { if (dt > 50) { car.readAndCopyInputs(); - vel_est.data = car.getVelocityEstimate() + vel_est.vel_est = car.getVelocityEstimate(); + vel_est.stamp = ros::Time::now(); pub_vel_est.publish(&vel_est); // publish estimated velocity ////////////////////////////////////////////////!!!! diff --git a/eval_sim/eval_run.jl b/eval_sim/eval_data.jl similarity index 75% rename from eval_sim/eval_run.jl rename to eval_sim/eval_data.jl index e2637741..a64a07f7 100644 --- a/eval_sim/eval_run.jl +++ b/eval_sim/eval_data.jl @@ -21,56 +21,86 @@ type Measurements{T} z::Array{T} # measurement values end +const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" # data from MPC +const log_path_sim = "$(homedir())/simulations/output.jld" # data from barc_simulation +const log_path_record = "$(homedir())/simulations/record-2016-10-18-16-53-56.jld" # data from barc_record +#const log_path_profile = "$(homedir())/simulations/profile.jlprof" -#const log_path = "$(homedir())/simulations/record-2016-10-13-20-03-58.jld" -const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" +# THIS FUNCTION EVALUATES DATA THAT WAS LOGGED BY THE SIMULATOR (INCLUDES "REAL" SIMULATION DATA) +# *********************************************************************************************** +function eval_sim() + d_sim = load(log_path_sim) + d_rec = load(log_path_record) -function initPlot() - linewidth = 0.4 - rc("axes", linewidth=linewidth) - rc("lines", linewidth=linewidth, markersize=2) - #rc("font", family="") - rc("axes", titlesize="small", labelsize="small") # can be set in Latex - rc("xtick", labelsize="x-small") - rc("xtick.major", width=linewidth/2) - rc("ytick", labelsize="x-small") - rc("ytick.major", width=linewidth/2) - rc("legend", fontsize="small") - rc("font",family="serif") - rc("font",size=8) - rc("figure",figsize=[4.5,3]) - #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") -end + imu_meas = d_sim["imu_meas"] + gps_meas = d_sim["gps_meas"] + z = d_sim["z"] + cmd_log = d_sim["cmd_log"] + slip_a = d_sim["slip_a"] + pos_info = d_rec["pos_info"] + vel_est = d_rec["vel_est"] -function simModel(z,u,dt,l_A,l_B) + t0 = pos_info.t[1] + track = create_track(0.3) - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle + figure() + ax1=subplot(311) + plot(z.t-t0,z.z,"-*") + title("Real states") + grid() + legend(["x","y","v_x","v_y","psi","psi_dot","a","d_f"]) + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-*") + title("Inputs") + grid() + legend(["u","d_f"]) + subplot(313,sharex=ax1) + plot(slip_a.t-t0,slip_a.z,"-*") + title("Slip angles") + grid() + legend(["a_f","a_r"]) - bta = atan(l_A/(l_A+l_B)*tan(u[2])) + figure() + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid(1) + title("x-y-view") + axis("equal") + legend(["Real state","GPS meas","Estimate"]) + + figure() + title("Comparison of psi") + plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],pos_info.t,pos_info.z[:,10:11],"-*") + legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) + grid() - zNext = z - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + figure() + title("Comparison of v") + plot(z.t,z.z[:,3:4],pos_info.t,pos_info.z[:,8:9],"-*",vel_est.t,vel_est.z) + legend(["real_xDot","real_yDot","est_xDot","est_yDot","v_x_meas"]) + grid() - return zNext + figure() + title("Comparison of x,y") + plot(z.t,z.z[:,1:2],pos_info.t,pos_info.z[:,6:7],"-*",gps_meas.t,gps_meas.z) + legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) + grid() end -function eval_run(log_path) - d = load(log_path) +# THIS FUNCTION EVALUATES DATA THAT WAS RECORDED BY BARC_RECORD.JL +# **************************************************************** +function eval_run(log_path_record::AbstractString) + d_rec = load(log_path_record) - imu_meas = d["imu_meas"] - gps_meas = d["gps_meas"] - cmd_log = d["cmd_log"] - vel_est = d["vel_est"] - pos_info = d["pos_info"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] - t0 = pos_info.t[1] - track = create_track(0.3) + t0 = pos_info.t[1] + track = create_track(0.3) figure() plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*") @@ -120,8 +150,10 @@ function eval_run(log_path) nothing end +# THIS FUNCTION EVALUATES MPC-SPECIFIC DATA +# ***************************************** function eval_LMPC() - d_sim = load(log_path) + d_rec = load(log_path_record) d_lmpc = load(log_path_LMPC) oldTraj = d_lmpc["oldTraj"] @@ -135,16 +167,16 @@ function eval_LMPC() c_Vy = d_lmpc["c_Vy"] c_Psi = d_lmpc["c_Psi"] cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC - step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] s_start = d_lmpc["s_start"] - imu_meas = d_sim["imu_meas"] - gps_meas = d_sim["gps_meas"] - cmd_log = d_sim["cmd_log"] # this is the command how it was received by the simulator - pos_info = d_sim["pos_info"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] t0 = t[1] @@ -156,17 +188,19 @@ function eval_LMPC() figure() ax1=subplot(311) - plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-o") - legend(["x_dot_est","x_dot_MPC"]) + plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*") + title("Comparison of publishing and receiving time") + legend(["x_dot_estimate","x_dot_MPC"]) grid("on") xlabel("t [s]") ylabel("v_x [m/s]") subplot(312,sharex=ax1) plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*") - legend(["a","d_f","a","d_f"]) + legend(["a_rec","d_f_rec","a_MPC","d_f_MPC"]) grid("on") subplot(313,sharex=ax1) plot(t-t0,c_Vx) + title("System ID xDot coefficients") grid("on") legend(["1","2","3"]) @@ -191,11 +225,13 @@ function eval_LMPC() track = create_track(0.3) figure() hold(1) - plot(x_est[:,1],x_est[:,2],"-o") - legend(["Estimated position"]) + plot(x_est[:,1],x_est[:,2],"-*") + title(["Estimated position"]) plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") axis("equal") grid(1) + # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA: + # CURRENT HEADING (PLOTTED BY A LINE) # for i=1:size(x_est,1) # #dir = [cos(x_est[i,3]) sin(x_est[i,3])] # #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] @@ -203,6 +239,8 @@ function eval_LMPC() # #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] # #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") # end + + # PREDICTED PATH for i=1:4:size(x_est,1) z_pred = zeros(11,4) z_pred[1,:] = x_est[i,:] @@ -212,6 +250,7 @@ function eval_LMPC() plot(z_pred[:,1],z_pred[:,2],"-*") end + # PREDICTED REFERENCE PATH (DEFINED BY POLYNOM) # for i=1:size(x_est,1) # s = 0.4:.1:2.5 # ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] @@ -229,6 +268,7 @@ function eval_LMPC() # for i=100:5:500 # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") # end + figure() plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-o") legend(["v_x","v_y","psiDot","ePsi","eY"]) @@ -251,6 +291,7 @@ function eval_LMPC() figure() ax1=subplot(211) + title("States and inputs") plot(t-t0,state[:,1:5]) legend(["v_x","v_y","psiDot","ePsi","eY"]) grid(1) @@ -354,6 +395,11 @@ function anim_LMPC(k1,k2) end end + +# ***************************************************************** +# ****** HELPER FUNCTIONS ***************************************** +# ***************************************************************** + function anim_MPC(z) figure() hold(0) @@ -455,3 +501,37 @@ function add_curve(theta::Array{Float64}, length::Int64, angle) push!(theta, theta[end] + d_theta) end end + +function initPlot() + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function simModel(z,u,dt,l_A,l_B) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + + return zNext +end diff --git a/eval_sim/eval_sim.jl b/eval_sim/eval_sim.jl deleted file mode 100644 index 3c98804c..00000000 --- a/eval_sim/eval_sim.jl +++ /dev/null @@ -1,438 +0,0 @@ -using JLD -using PyPlot -using HDF5, JLD, ProfileView - -include("../workspace/src/barc/src/LMPC_lib/classes.jl") - -type Measurements{T} - i::Int64 # measurement counter - t::Array{Float64} # time data - z::Array{T} # measurement values -end - - -const log_path = "$(homedir())/simulations/output.jld" -const log_path_record = "$(homedir())/simulations/record-2016-10-14-09-22-12.jld" -const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" -const log_path_profile = "$(homedir())/simulations/profile.jlprof" - - -function simModel(z,u,dt,l_A,l_B) - - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - - zNext = z - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v - - return zNext -end - -function eval_sim() - d = load(log_path) - d2 = load(log_path_record) - - imu_meas = d["imu_meas"] - gps_meas = d["gps_meas"] - z = d["z"] - cmd_log = d["cmd_log"] - slip_a = d["slip_a"] - pos_info = d2["pos_info"] - vel_est = d2["vel_est"] - - t0 = pos_info.t[1] - track = create_track(0.3) - - figure() - ax1=subplot(311) - plot(z.t-t0,z.z,"-o") - grid() - legend(["x","y","v_x","v_y","psi","psi_dot","a","d_f"]) - subplot(312,sharex=ax1) - plot(cmd_log.t-t0,cmd_log.z,"-o") - grid() - legend(["u","d_f"]) - subplot(313,sharex=ax1) - plot(slip_a.t-t0,slip_a.z,"-o") - grid() - legend(["a_f","a_r"]) - - figure() - plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") - plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - grid(1) - title("x-y-view") - axis("equal") - legend(["Real state","GPS meas","estimate"]) - - figure() - title("Comparison of psi") - plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],pos_info.t,pos_info.z[:,10:11],"-*") - legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) - grid() - - figure() - title("Comparison of v") - plot(z.t,z.z[:,3:4],pos_info.t,pos_info.z[:,8:9],"-*",vel_est.t,vel_est.z) - legend(["real_xDot","real_yDot","est_xDot","est_yDot","v_x_meas"]) - grid() - - figure() - title("Comparison of x,y") - plot(z.t,z.z[:,1:2],pos_info.t,pos_info.z[:,6:7],"-*",gps_meas.t,gps_meas.z) - legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) - grid() - - #figure() - #plot(est.z[:,1],est.z[:,2],"x",est_dyn.z[:,1],est_dyn.z[:,2],"-*",z.z[:,1],z.z[:,2],"-") - #grid(1) - #legend(["est","est_dyn","true"]) - - #figure() - #plot(z.t-t0,z.z[:,5],imu_meas.t-t0,imu_meas.z,est.t-t0,est.z[:,3]) - #grid(1) - #legend(["Real psi","psi meas","estimate"]) - - figure() - plot(cmd_log.t-t0,cmd_log.z) - legend(["a","d_f"]) - grid() -end - -function eval_LMPC() - d_sim = load(log_path) - d_lmpc = load(log_path_LMPC) - - oldTraj = d_lmpc["oldTraj"] - t = d_lmpc["t"] - state = d_lmpc["state"] - sol_z = d_lmpc["sol_z"] - sol_u = d_lmpc["sol_u"] - cost = d_lmpc["cost"] - curv = d_lmpc["curv"] - c_Vx = d_lmpc["c_Vx"] - c_Vy = d_lmpc["c_Vy"] - c_Psi = d_lmpc["c_Psi"] - cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC - step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC - - x_est = d_lmpc["x_est"] - coeffX = d_lmpc["coeffX"] - coeffY = d_lmpc["coeffY"] - s_start = d_lmpc["s_start"] - est = d_sim["estimate"] - est_dyn = d_sim["estimate_dyn"] - imu_meas = d_sim["imu_meas"] - gps_meas = d_sim["gps_meas"] - z = d_sim["z"] - cmd_log = d_sim["cmd_log"] # this is the command how it was received by the simulator - - t0 = t[1] - - figure() - plot(t-t0,step_diff) - grid("on") - title("One-step-errors") - legend(["xDot","yDot","psiDot","ePsi","eY"]) - - figure() - ax1=subplot(311) - plot(z.t-t0,z.z[:,3],"--",est_dyn.t-t0,est_dyn.z[:,3],".",t-t0,state[:,1],"-o") - legend(["x_dot_real","x_dot_est","x_dot_MPC"]) - grid("on") - xlabel("t [s]") - ylabel("v_x [m/s]") - subplot(312,sharex=ax1) - plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*") - legend(["a","d_f","a","d_f"]) - grid("on") - subplot(313,sharex=ax1) - plot(t-t0,c_Vx) - grid("on") - legend(["1","2","3"]) - - figure() - c = zeros(size(curv,1),1) - for i=1:size(curv,1) - s = state[i,1] - c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] - end - plot(s_start+state[:,1],c,"-o") - for i=1:2:size(curv,1) - s = sol_z[:,1,i] - c = zeros(size(curv,1),1) - c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - plot(s_start[i]+s,c,"-*") - end - title("Curvature over path") - xlabel("Curvilinear abscissa [m]") - ylabel("Curvature") - grid() - - track = create_track(0.3) - figure() - hold(1) - plot(x_est[:,1],x_est[:,2],"-o") - legend(["Estimated position"]) - plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - axis("equal") - grid(1) - # for i=1:size(x_est,1) - # #dir = [cos(x_est[i,3]) sin(x_est[i,3])] - # #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] - # #lin = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir] - # #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] - # #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") - # end - for i=1:4:size(x_est,1) - z_pred = zeros(11,4) - z_pred[1,:] = x_est[i,:] - for j=2:11 - z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) - end - plot(z_pred[:,1],z_pred[:,2],"-*") - end - - # for i=1:size(x_est,1) - # s = 0.4:.1:2.5 - # ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - # x = ss*coeffX[i,:]' - # y = ss*coeffY[i,:]' - # plot(x,y) - # end - - # rg = 100:500 - # figure() - # plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") - # title("Comparison states and prediction") - # legend(["ey","epsi","v"]) - # grid(1) - # for i=100:5:500 - # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") - # end - figure() - plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-o") - legend(["v_x","v_y","psiDot","ePsi","eY"]) - grid(1) - - figure() - plot(s_start+state[:,1],state[:,[2,4]],"*") - grid() - - figure() - ax1=subplot(211) - plot(t-t0,state,z.t-t0,z.z[:,3:6],"--") - legend(["v_x","v_y","psiDot","ePsi","eY","s","real_v_x","real_v_y","real_psi","real_psiDot"]) - grid(1) - subplot(212,sharex = ax1) - plot(t-t0,cost) - grid(1) - legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) - # figure() - # plot(1:size(curv,1),curv) - # grid() - # title("Polynomial coefficients") - # legend(["1","2","3","4","5","6","7","8","9"]) -end - -function eval_oldTraj(i) - d = load(log_path_LMPC) - oldTraj = d["oldTraj"] - plot(oldTraj[:,1,1,i],oldTraj[:,2:4,1,i],"-o",oldTraj[:,1,2,i],oldTraj[:,2:4,2,i],"-*") -end - -function eval_LMPC_coeff(k) - d = load(log_path_LMPC) - oldTraj = d["oldTraj"] - sol_z = d["sol_z"] - sol_u = d["sol_u"] - coeffCost = d["coeffCost"] - coeffConst = d["coeffConst"] - s_start = d["s_start"] - - s = sol_z[:,1,k] - ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - subplot(311) - plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) - grid() - title("Position = $(s_start[k] + s[1]), k = $k") - xlabel("s") - ylabel("e_Y") - subplot(312) - plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) - grid() - xlabel("s") - ylabel("e_Psi") - subplot(313) - plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) - grid() - xlabel("s") - ylabel("v") -end - -function anim_LMPC(k1,k2) - d = load(log_path_LMPC) - oldTraj = d["oldTraj"] - sol_z = d["sol_z"] - sol_u = d["sol_u"] - coeffCost = d["coeffCost"] - coeffConst = d["coeffConst"] - s_start = d["s_start"] - state = d["state"] - - N = size(sol_z,1)-1 - - for k=k1:k2 - s = sol_z[1:10,1,k] - subplot(211) - plot(s,sol_u[:,1,k],"-o") - ylim([-1,2]) - xlim([0,2]) - grid() - subplot(212) - plot(s,sol_u[:,2,k],"-o") - ylim([-0.5,0.5]) - xlim([0,2]) - grid() - sleep(0.1) - end - - figure() - hold(0) - for k=k1:k2 - s_state = s_start[k:k+N] + state[k:k+N,1] - s_start[k] - s = sol_z[:,1,k] - ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - subplot(311) - plot(s,sol_z[:,2,k],"-o",s_state,state[k:k+N,2],"-*",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) - grid() - title("Position = $(s_start[k] + s[1]), k = $k") - xlabel("s") - xlim([0,3]) - ylim([-0.2,0.2]) - ylabel("e_Y") - subplot(312) - plot(s,sol_z[:,3,k],"-o",s_state,state[k:k+N,3],"-*",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) - grid() - xlabel("s") - ylabel("e_Psi") - xlim([0,3]) - ylim([-0.2,0.2]) - subplot(313) - plot(s,sol_z[:,4,k],"-o",s_state,state[k:k+N,4],"-*",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) - grid() - xlabel("s") - ylabel("v") - xlim([0,3]) - ylim([0,2]) - sleep(0.1) - end -end - -function anim_MPC(z) - figure() - hold(0) - grid(1) - for i=1:size(z,3) - plot(z[:,:,i]) - xlim([1,11]) - ylim([-2,2]) - sleep(0.01) - end -end - -function anim_curv(curv) - s = 0.0:.05:2.0 - figure() - hold(0) - #ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - for i=1:size(curv,1) - c = ss*curv[i,:]' - plot(s,c) - xlim([0,2]) - ylim([-1.5,1.5]) - sleep(0.1) - end -end - - - -function eval_prof() - Profile.clear() - @load "$(homedir())/simulations/profile.jlprof" - ProfileView.view(li, lidict=lidict) -end - -function create_track(w) - x = [0.0] # starting point - y = [0.0] - x_l = [0.0] # starting point - y_l = [w] - x_r = [0.0] # starting point - y_r = [-w] - ds = 0.06 - - theta = [0.0] - - # SOPHISTICATED TRACK - # add_curve(theta,30,0.0) - # add_curve(theta,60,-2*pi/3) - # add_curve(theta,90,pi) - # add_curve(theta,80,-5*pi/6) - # add_curve(theta,10,0.0) - # add_curve(theta,50,-pi/2) - # add_curve(theta,50,0.0) - # add_curve(theta,40,-pi/4) - # add_curve(theta,30,pi/4) - # add_curve(theta,20,0.0) - # add_curve(theta,50,-pi/2) - # add_curve(theta,25,0.0) - # add_curve(theta,50,-pi/2) - # add_curve(theta,28,0.0) - - # # SIMPLE track - # add_curve(theta,50,0) - # add_curve(theta,100,-pi) - # add_curve(theta,100,0) - # add_curve(theta,100,-pi) - # add_curve(theta,49,0) - - # SHORT SIMPLE track - add_curve(theta,10,0) - add_curve(theta,80,-pi) - add_curve(theta,20,0) - add_curve(theta,80,-pi) - add_curve(theta,9,0) - - for i=1:length(theta) - push!(x, x[end] + cos(theta[i])*ds) - push!(y, y[end] + sin(theta[i])*ds) - push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) - push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) - push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) - push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) - end - track = cat(2, x, y, x_l, y_l, x_r, y_r) - return track - #plot(x,y,x_l,y_l,x_r,y_r) -end - -function add_curve(theta::Array{Float64}, length::Int64, angle) - d_theta = 0 - curve = 2*sum(1:length/2)+length/2 - for i=0:length-1 - if i < length/2+1 - d_theta = d_theta + angle / curve - else - d_theta = d_theta - angle / curve - end - push!(theta, theta[end] + d_theta) - end -end diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index ef279143..48e541fa 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -54,6 +54,7 @@ add_message_files( ECU.msg Z_KinBkMdl.msg Z_DynBkMdl.msg + Vel_est.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 088f68e7..6c7a885c 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -16,7 +16,7 @@ - + diff --git a/workspace/src/barc/launch/run_lmpc_barc.launch b/workspace/src/barc/launch/run_lmpc_barc.launch index fa0bdddd..fce0bfa8 100644 --- a/workspace/src/barc/launch/run_lmpc_barc.launch +++ b/workspace/src/barc/launch/run_lmpc_barc.launch @@ -16,8 +16,8 @@ - - + + diff --git a/workspace/src/barc/msg/Vel_est.msg b/workspace/src/barc/msg/Vel_est.msg new file mode 100644 index 00000000..10d26b83 --- /dev/null +++ b/workspace/src/barc/msg/Vel_est.msg @@ -0,0 +1,2 @@ +float32 vel_est +time stamp \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 1bf25c4e..f3f89eed 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -191,7 +191,7 @@ function main() # Solve the MPC problem tic() if lapStatus.currentLap <= 2 - z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],zCurr[i,1]] # use kinematic model and its states + z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,last_u') else # otherwise: use system-ID-model solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index c4e2b467..5ef806e5 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -10,7 +10,7 @@ Position info (/pos_info) =# using RobotOS -@rosimport barc.msg: ECU, pos_info +@rosimport barc.msg: ECU, pos_info, Vel_est @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @@ -76,10 +76,10 @@ function pos_info_callback(msg::pos_info,pos_info_log::Measurements) nothing end -function vel_est_callback(msg::Float32Msg,vel_est_log::Measurements) +function vel_est_callback(msg::Vel_est,vel_est_log::Measurements) vel_est_log.i += 1 vel_est_log.t[vel_est_log.i] = time() - vel_est_log.z[vel_est_log.i] = convert(Float64,msg.data) + vel_est_log.z[vel_est_log.i] = convert(Float64,msg.vel_est) nothing end @@ -103,7 +103,7 @@ function main() s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} - s6 = Subscriber("vel_est", Float32Msg, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{std_msgs.msg.Float32Msg} + s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} dt = 0.1 loop_rate = Rate(1/dt) diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 0ee7230c..8c787b79 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -14,7 +14,7 @@ =# using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl +@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl, Vel_est @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @@ -72,7 +72,7 @@ function main() #pub_enc = Publisher("encoder", Encoder, queue_size=1)::RobotOS.Publisher{barc.msg.Encoder} pub_gps = Publisher("hedge_pos", hedge_pos, queue_size=1)::RobotOS.Publisher{marvelmind_nav.msg.hedge_pos} pub_imu = Publisher("imu/data", Imu, queue_size=1)::RobotOS.Publisher{sensor_msgs.msg.Imu} - pub_vel = Publisher("vel_est", Float32Msg, queue_size=1)::RobotOS.Publisher{std_msgs.msg.Float32Msg} + pub_vel = Publisher("vel_est", Vel_est, queue_size=1)::RobotOS.Publisher{barc.msg.Vel_est} s1 = Subscriber("ecu", ECU, ECU_callback, (u_current,cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} @@ -111,7 +111,7 @@ function main() println("Publishing sensor information. Simulator running.") imu_data = Imu() - vel_est = Float32Msg() + vel_est = Vel_est() t0 = time() t = 0.0 @@ -121,6 +121,7 @@ function main() while ! is_shutdown() t = time() + t_ros = get_rostime() # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:],u_current', dt, modelParams) z_real.t[i] = t @@ -136,6 +137,7 @@ function main() imu_meas.z[imu_meas.i,:] = [yaw psiDot] imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) + imu_data.header.stamp = t_ros publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end @@ -143,9 +145,11 @@ function main() # Velocity measurements if i%5 == 0 # 20 Hz if norm(z_current[i,1:2][:]-vel_pos) > vel_dist_update # only update if a magnet has passed the sensor - vel_est.data = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) + #vel_est.data = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) + vel_est.vel_est = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) vel_pos = z_current[i,1:2][:] end + vel_est.stamp = t_ros publish(pub_vel, vel_est) end @@ -153,10 +157,10 @@ function main() if i % 4 == 0 # 25 Hz x = round(z_current[i,1] + 0.02*randn(),2) # Indoor gps measures, rounded on cm y = round(z_current[i,2] + 0.02*randn(),2) - if randn()>3 # simulate gps-outlier (probability about 0.13% for randn()>3, ) - x += randn() # add random value to x and y - y += randn() - sim_gps_interrupt = 4 # also do a little interruption + if randn()>2.5 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) + x += 1#randn() # add random value to x and y + y -= 1#randn() + #sim_gps_interrupt = 6 # also do a little interruption elseif randn()>3 && sim_gps_interrupt < 0 sim_gps_interrupt = 10 # simulate gps-interrupt (10 steps at 25 Hz is 0.4 seconds) end @@ -164,7 +168,7 @@ function main() gps_meas.i += 1 gps_meas.t[gps_meas.i] = t gps_meas.z[gps_meas.i,:] = [x y] - gps_data = hedge_pos(0,x,y,0,0) + gps_data = hedge_pos(0,convert(Float64,get_rostime()),x,y,0,0) publish(pub_gps, gps_data) end sim_gps_interrupt -= 1 diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index cb4b253b..a5e79b54 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -17,7 +17,7 @@ import time import os from Localization_helpers import Localization -from barc.msg import ECU, Encoder, Z_DynBkMdl, pos_info +from barc.msg import ECU, Encoder, Z_DynBkMdl, pos_info, Vel_est from sensor_msgs.msg import Imu from geometry_msgs.msg import Vector3 from marvelmind_nav.msg import hedge_pos @@ -27,8 +27,11 @@ from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_psi_drift, h_KinBkMdl_psi_drift from tf import transformations from numpy import unwrap +from numpy import * # input variables +cmd_servo = 0 +cmd_motor = 0 d_f = 0 FxR = 0 @@ -37,35 +40,62 @@ yaw0 = 0 # yaw at t = 0 yaw = 0 (roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) +imu_times = [0]*25 +psiDot_hist = [0]*25 +# Velocity +vel_est = 0 +vel_est_hist = [0]*2 +vel_times = [0]*2 + +# GPS x_meas = 0 y_meas = 0 -vel_est = 0 +x_hist = [0]*30 +y_hist = [0]*30 +gps_times = [0]*30 + +poly_x = [0]*3 # polynomial coefficients for x and y position measurement (2nd order) +poly_y = [0]*3 -running = False +t0 = 0 +running = False # ecu command update def ecu_callback(data): - global FxR, d_f, running - FxR = data.motor # input motor force [N] - d_f = data.servo # input steering angle [rad] + global FxR, cmd_motor, cmd_servo, running + cmd_motor = data.motor # input motor force [N] + cmd_servo = data.servo # input steering angle [rad] + FxR = cmd_motor if not running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero running = True # ultrasound gps data def gps_callback(data): # units: [rad] and [rad/s] - global x_meas, y_meas - x_meas = data.x_m # data is given in cm - y_meas = data.y_m + global x_meas, y_meas, t0 + x_meas_pred = polyval(poly_x,data.timestamp_ros-t0) # predict new position + y_meas_pred = polyval(poly_y,data.timestamp_ros-t0) + + if abs(x_meas_pred-data.x_m) < 0.5 and abs(y_meas_pred-data.y_m) < 0.5 or not running: # check for outlier + x_meas = data.x_m # data is given in cm + y_meas = data.y_m + + x_hist.append(x_meas) + y_hist.append(y_meas) + gps_times.append(data.timestamp_ros-t0) + x_hist.pop(0) + y_hist.pop(0) + gps_times.pop(0) # imu measurement update def imu_callback(data): # units: [rad] and [rad/s] global roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z global yaw_prev, yaw0, yaw - + global imu_times, psiDot_hist + # get orientation from quaternion data, and convert to roll, pitch, yaw # extract angular velocity and linear acceleration data ori = data.orientation @@ -80,6 +110,9 @@ def imu_callback(data): yaw = 0 # and current yaw to zero else: yaw = yaw - yaw0 + + imu_times.append(data.header.stamp.to_sec()-t0) + imu_times.pop(0) # extract angular velocity and linear acceleration data w_x = data.angular_velocity.x @@ -89,18 +122,27 @@ def imu_callback(data): a_y = data.linear_acceleration.y a_z = data.linear_acceleration.z + psiDot_hist.append(w_z) + psiDot_hist.pop(0) + def vel_est_callback(data): - global vel_est - vel_est = data.data + global vel_est, t0, vel_est_hist, vel_times + if not data.vel_est == vel_est or not running: # if we're receiving a new measurement + vel_est = data.vel_est + vel_est_hist.append(vel_est) + vel_est_hist.pop(0) + vel_times.append(data.stamp.to_sec()-t0) + vel_times.pop(0) # state estimation node def state_estimation(): + global t0, poly_x, poly_y # initialize node rospy.init_node('state_estimation', anonymous=True) # topic subscriptions / publications rospy.Subscriber('imu/data', Imu, imu_callback) - rospy.Subscriber('vel_est', Float32, vel_est_callback) + rospy.Subscriber('vel_est', Vel_est, vel_est_callback) rospy.Subscriber('ecu', ECU, ecu_callback) rospy.Subscriber('hedge_pos', hedge_pos, gps_callback) state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 1) @@ -122,7 +164,7 @@ def state_estimation(): loop_rate = 25 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) - t0 = time.time() + t0 = rospy.get_rostime().to_sec() # settings about psi estimation (use different models accordingly) psi_drift_active = True @@ -157,11 +199,53 @@ def state_estimation(): l.create_track() l.prepare_trajectory(0.06) - w_z_f = 0 # filtered w_z (= angular velocity psiDot) psi_prev = 0 # for debugging + d_f = 0 + while not rospy.is_shutdown(): - # publish state estimate + t = rospy.get_rostime().to_sec()-t0 # current time + + # calculate new steering angle (low pass filter on steering input) + d_f = d_f + (cmd_servo-d_f)*0.25 + + # update x and y polynomial: + t_matrix = vstack((gps_times,gps_times,ones(size(gps_times)))) + t_matrix[0] = t_matrix[0]**2 + poly_x = linalg.lstsq(t_matrix.T,x_hist)[0] + poly_y = linalg.lstsq(t_matrix.T,y_hist)[0] + # calculate current position from interpolated measurements + x_meas_pred = polyval(poly_x,t) + y_meas_pred = polyval(poly_y,t) + + # print "Times:" + # print gps_times + # print "x-values:" + # print x_hist + # print "ROS time: %f"%t + # print "Polynomial coefficients:" + # print poly_x + # print "tdif = %f"%(t-gps_times[-1]) + # print "Pred. x_meas: %f, real x_meas: %f"%(x_meas_pred,x_meas) + + # update velocity estimation polynomial: + t_matrix_vel = vstack((vel_times,ones(size(vel_times)))) + poly_vel = linalg.lstsq(t_matrix_vel.T,vel_est_hist)[0] + vel_est_pred = polyval(poly_vel,t) + # print "Times:" + # print vel_times + # print "Meas:" + # print vel_est_hist + # print "ROS time: %f"%t + # print "tdif = %f"%(t-vel_times[-1]) + # print "Pred. vel: %f, real vel_est: %f"%(vel_est_pred,vel_est) + + # update IMU polynomial: + t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) + t_matrix_imu[0] = t_matrix_imu[0]**2 + poly_psiDot = linalg.lstsq(t_matrix_imu.T,psiDot_hist)[0] + psiDot_meas_pred = polyval(poly_psiDot,t) + if psi_drift_active: (x,y,psi,v,psi_drift) = z_EKF # note, r = EKF estimate yaw rate else: @@ -177,9 +261,8 @@ def state_estimation(): v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) v_x_pred = cos(bta)*v_pred v_y_pred = sin(bta)*v_pred - w_z_f = w_z_f + 0.4*(w_z-w_z_f) # simple low pass filter on angular velocity - psi_dot_pred = w_z_f + psi_dot_pred = psiDot_meas_pred # Update track position l.set_pos(x_pred,y_pred,psi_pred,v_x_pred,v_x_pred,v_y_pred,psi_dot_pred) # v = v_x @@ -191,7 +274,7 @@ def state_estimation(): # apply EKF # get measurement if est_mode==1: - y = array([x_meas,y_meas,yaw,vel_est]) + y = array([x_meas_pred,y_meas_pred,yaw,vel_est]) elif est_mode==2: y = array([yaw,vel_est]) elif est_mode==3: diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg index 84a917dc..fbfd84ac 100644 --- a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg @@ -1,4 +1,5 @@ int64 timestamp_ms +float64 timestamp_ros float64 x_m float64 y_m float64 z_m diff --git a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp index 3469fac4..cc86ef94 100644 --- a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp +++ b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp @@ -57,6 +57,7 @@ static bool hedgeReceiveCheck(void) hedge_pos_msg.timestamp_ms= position.timestamp*15.625;// alpha-cycles ==> msec } + hedge_pos_msg.timestamp_ros= ros::Time::now().toSec(); // added by mb for custom hedge_pos msg hedge_pos_msg.x_m= position.x/100.0; hedge_pos_msg.y_m= position.y/100.0; hedge_pos_msg.z_m= position.z/100.0; @@ -87,6 +88,7 @@ int main(int argc, char **argv) // default values for position message hedge_pos_msg.timestamp_ms = 0; + hedge_pos_msg.timestamp_ros = 0; // added by mb hedge_pos_msg.x_m = 0.0; hedge_pos_msg.y_m = 0.0; hedge_pos_msg.z_m = 0.0; @@ -102,9 +104,10 @@ int main(int argc, char **argv) if (hedgeReceiveCheck()) {// hedgehog data received - ROS_INFO("%d, %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + ROS_INFO("%d, %d, %.3f, X=%.3f Y= %.3f Z=%.3f flags=%d", (int) hedge_pos_msg.timestamp_ms, (int) (hedge_pos_msg.timestamp_ms - hedge_timestamp_prev), + (float) hedge_pos_msg.timestamp_ros, (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, (int) hedge_pos_msg.flags); hedge_pos_publisher.publish(hedge_pos_msg); diff --git a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c index 205f2117..9bd2b103 100644 --- a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c +++ b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c @@ -93,7 +93,7 @@ HANDLE OpenSerialPort_ (const char * portFileName, uint32_t baudrate, if (verbose) puts ("Error: unable to set serial port parameters"); CloseHandle (ttyHandle); return INVALID_HANDLE_VALUE; - } + } return ttyHandle; } #else From 3b2f76ba11e9567c93b96e5b9f302746926118fb Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 19 Oct 2016 15:53:45 -0700 Subject: [PATCH 068/183] Cleaned code (first time working with Lint)2 --- eval_sim/eval_data.jl | 22 +- workspace/src/barc/src/LMPC_lib | 1 - workspace/src/barc/src/LMPC_node.jl | 15 +- .../src/barc/src/Localization_helpers.py | 11 +- workspace/src/barc/src/barc_lib | 1 + workspace/src/barc/src/barc_record.jl | 7 +- workspace/src/barc/src/barc_simulator.jl | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 23 +- .../src/barc/src/state_estimation_DynBkMdl.py | 207 ++++++++---------- .../src/marvelmind_indoor_gps/CMakeLists.txt | 187 ---------------- .../launch/indoor_gps.launch | 6 - .../src/marvelmind_indoor_gps/package.xml | 14 -- .../marvelmind_indoor_gps/src/indoor_gps.py | 78 ------- 13 files changed, 133 insertions(+), 441 deletions(-) delete mode 160000 workspace/src/barc/src/LMPC_lib create mode 160000 workspace/src/barc/src/barc_lib delete mode 100644 workspace/src/marvelmind_indoor_gps/CMakeLists.txt delete mode 100644 workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch delete mode 100644 workspace/src/marvelmind_indoor_gps/package.xml delete mode 100755 workspace/src/marvelmind_indoor_gps/src/indoor_gps.py diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index a64a07f7..2f513cea 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -13,7 +13,7 @@ using HDF5, JLD, ProfileView # pos_info[10] = psi # pos_info[11] = psiDot -include("../workspace/src/barc/src/LMPC_lib/classes.jl") +include("../workspace/src/barc/src/barc_lib/classes.jl") type Measurements{T} i::Int64 # measurement counter @@ -24,12 +24,13 @@ end const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" # data from MPC const log_path_sim = "$(homedir())/simulations/output.jld" # data from barc_simulation const log_path_record = "$(homedir())/simulations/record-2016-10-18-16-53-56.jld" # data from barc_record -#const log_path_profile = "$(homedir())/simulations/profile.jlprof" # THIS FUNCTION EVALUATES DATA THAT WAS LOGGED BY THE SIMULATOR (INCLUDES "REAL" SIMULATION DATA) # *********************************************************************************************** -function eval_sim() +function eval_sim(code::AbstractString) + log_path_sim = "$(homedir())/simulations/output-SIM-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_sim = load(log_path_sim) d_rec = load(log_path_record) @@ -90,7 +91,8 @@ end # THIS FUNCTION EVALUATES DATA THAT WAS RECORDED BY BARC_RECORD.JL # **************************************************************** -function eval_run(log_path_record::AbstractString) +function eval_run(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) imu_meas = d_rec["imu_meas"] @@ -152,7 +154,9 @@ end # THIS FUNCTION EVALUATES MPC-SPECIFIC DATA # ***************************************** -function eval_LMPC() +function eval_LMPC(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) d_lmpc = load(log_path_LMPC) @@ -301,10 +305,14 @@ function eval_LMPC() legend(["u","d_f"]) end -function eval_oldTraj(i) +function eval_oldTraj(code::AbstractString,i::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" d = load(log_path_LMPC) oldTraj = d["oldTraj"] - plot(oldTraj[:,1,1,i],oldTraj[:,2:4,1,i],"-o",oldTraj[:,1,2,i],oldTraj[:,2:4,2,i],"-*") + t = d["t"] + plot(t,oldTraj[:,:,1,i],"-*") + grid("on") + legend(["v_x","v_x","psiDot","ePsi","eY","s"]) end function eval_LMPC_coeff(k) diff --git a/workspace/src/barc/src/LMPC_lib b/workspace/src/barc/src/LMPC_lib deleted file mode 160000 index 41e1bb96..00000000 --- a/workspace/src/barc/src/LMPC_lib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 41e1bb964699bf98c6d13ad038001232f1810528 diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index f3f89eed..6ca9ffd7 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -12,11 +12,11 @@ using JuMP using Ipopt using JLD -include("LMPC_lib/classes.jl") -include("LMPC_lib/MPC_models.jl") -include("LMPC_lib/coeffConstraintCost.jl") -include("LMPC_lib/solveMpcProblem.jl") -include("LMPC_lib/functions.jl") +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/coeffConstraintCost.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/LMPC/functions.jl") function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data @@ -90,6 +90,7 @@ function main() # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + run_id = get_param("run_id") println("Finished initialization.") # Lap parameters switchLap = false # initialize lap lap trigger @@ -250,12 +251,12 @@ function main() end # Save simulation data to file - log_path = "$(homedir())/simulations/output_LMPC.jld" + log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4]).jld" save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) - println("Exiting LMPC node. Saved data.") + println("Exiting LMPC node. Saved data to $log_path.") end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 9f9f6485..c58681a8 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -1,7 +1,13 @@ -from numpy import *#sin, cos, tan, arctan, array, dot, linspace, pi, ones, zeros, sum, amin, argmin +"""This module includes all functions that are necessary to convert the current absolute position +(x, y, psi) and the given racetrack to the current relative position (s, eY, ePsi)""" +#from numpy import sin, cos, tan, array, dot, linspace, pi, ones, zeros, amin, argmin, arange +#from numpy import hstack, vstack, sign, size, diff, cumsum, mod, floor, interp, linalg +#from numpy import polyval, polyder, arctan2 +from numpy import * import scipy.optimize -def create_circle(rad,n,c): +def create_circle(rad, n, c): + """This function creates a simple circular racetrack""" ang = linspace(0,2*pi,n) x = rad*cos(ang)+c[0] y = rad*sin(ang)+c[1] @@ -24,6 +30,7 @@ def create_bezier(p0,p1,p2,dt): return p class Localization: + """ This is the main class which includes all variables and functions to calculate the current relative position.""" n = 0 # number of nodes c = 0 # center of circle (in case of circular trajectory) pos = 0 # current position diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib new file mode 160000 index 00000000..c0a69ccb --- /dev/null +++ b/workspace/src/barc/src/barc_lib @@ -0,0 +1 @@ +Subproject commit c0a69ccb330a3d1d63a17d47224d16194dc382c9 diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 5ef806e5..9838c033 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -15,7 +15,6 @@ using RobotOS @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos -@rosimport std_msgs.msg: Float32 rostypegen() using barc.msg using data_service.msg @@ -105,8 +104,7 @@ function main() s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} - dt = 0.1 - loop_rate = Rate(1/dt) + run_id = get_param("run_id") println("Recorder running.") spin() # wait for callbacks until shutdown @@ -119,7 +117,8 @@ function main() clean_up(vel_est_log) # Save simulation data to file - log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" + #log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" + log_path = "$(homedir())/simulations/output-record-$(run_id[1:4]).jld" save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"pos_info",pos_info_log,"vel_est",vel_est_log) println("Exiting node... Saving recorded data to $log_path.") end diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl index d0294bc5..88ab6152 100755 --- a/workspace/src/barc/src/barc_simulator.jl +++ b/workspace/src/barc/src/barc_simulator.jl @@ -25,7 +25,7 @@ using geometry_msgs.msg using sensor_msgs.msg using JLD -include("LMPC_lib/simModel.jl") +include("barc_lib/simModel.jl") u_current = zeros(Float64,2,1) # msg ECU is Float32 ! diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 8c787b79..f676c0dd 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -14,12 +14,11 @@ =# using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl, Vel_est +@rosimport barc.msg: ECU, Vel_est @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos -@rosimport std_msgs.msg: Float32 rostypegen() using barc.msg using data_service.msg @@ -29,8 +28,8 @@ using std_msgs.msg using marvelmind_nav.msg using JLD -include("LMPC_lib/classes.jl") -include("LMPC_lib/simModel.jl") +include("barc_lib/classes.jl") +include("barc_lib/simModel.jl") # This type contains measurement data (time, values and a counter) type Measurements{T} @@ -69,7 +68,6 @@ function main() # initiate node, set up publisher / subscriber topics init_node("barc_sim") - #pub_enc = Publisher("encoder", Encoder, queue_size=1)::RobotOS.Publisher{barc.msg.Encoder} pub_gps = Publisher("hedge_pos", hedge_pos, queue_size=1)::RobotOS.Publisher{marvelmind_nav.msg.hedge_pos} pub_imu = Publisher("imu/data", Imu, queue_size=1)::RobotOS.Publisher{sensor_msgs.msg.Imu} pub_vel = Publisher("vel_est", Vel_est, queue_size=1)::RobotOS.Publisher{barc.msg.Vel_est} @@ -89,16 +87,11 @@ function main() last_updated = 0.0 r_tire = 0.036 # radius from tire center to perimeter along magnets [m] - quarterCirc = 0.5 * pi * r_tire # length of a quarter of a tire, distance from one to the next encoder - FL = 0 #front left wheel encoder counter - FR = 0 #front right wheel encoder counter - BL = 0 #back left wheel encoder counter - BR = 0 #back right wheel encoder counter - imu_drift = 0.0 # simulates yaw-sensor drift over time (slow sine) modelParams = ModelParams() + run_id = get_param("run_id") # modelParams.l_A = copy(get_param("L_a")) # always throws segmentation faults *after* execution!!! ?? # modelParams.l_B = copy(get_param("L_a")) # modelParams.m = copy(get_param("m")) @@ -145,7 +138,6 @@ function main() # Velocity measurements if i%5 == 0 # 20 Hz if norm(z_current[i,1:2][:]-vel_pos) > vel_dist_update # only update if a magnet has passed the sensor - #vel_est.data = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) vel_est.vel_est = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) vel_pos = z_current[i,1:2][:] end @@ -154,10 +146,10 @@ function main() end # GPS measurements - if i % 4 == 0 # 25 Hz + if i%4 == 0 # 25 Hz x = round(z_current[i,1] + 0.02*randn(),2) # Indoor gps measures, rounded on cm y = round(z_current[i,2] + 0.02*randn(),2) - if randn()>2.5 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) + if randn()>3 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) x += 1#randn() # add random value to x and y y -= 1#randn() #sim_gps_interrupt = 6 # also do a little interruption @@ -178,7 +170,6 @@ function main() end # Clean up buffers - clean_up(gps_meas) clean_up(imu_meas) clean_up(cmd_log) @@ -190,7 +181,7 @@ function main() clean_up(slip_a) # Save simulation data to file - log_path = "$(homedir())/simulations/output.jld" + log_path = "$(homedir())/simulations/output-SIM-$(run_id[1:4]).jld" save(log_path,"gps_meas",gps_meas,"z",z_real,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") #writedlm(log_path,z_current[1:i-1,:]) diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index a5e79b54..1f12a762 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -1,38 +1,32 @@ #!/usr/bin/env python # --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for +# Licensing Information: You are free to use or extend these projects for # education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link +# and (2) you provide clear attribution to UC Berkeley, including a link # to http://barc-project.com # # Attibution Information: The barc project ROS code-base was developed # at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales # (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was # based on an open source project by Bruce Wootton # --------------------------------------------------------------------------- import rospy -import time -import os from Localization_helpers import Localization -from barc.msg import ECU, Encoder, Z_DynBkMdl, pos_info, Vel_est +from barc.msg import ECU, pos_info, Vel_est from sensor_msgs.msg import Imu -from geometry_msgs.msg import Vector3 from marvelmind_nav.msg import hedge_pos -from std_msgs.msg import Float32 -from numpy import pi, cos, sin, eye, array, zeros, diag, arctan, tan, size, sign -from observers import kinematicLuembergerObserver, ekf +from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, size, sign +from numpy import unwrap, vstack, ones, linalg, polyval +from observers import ekf from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_psi_drift, h_KinBkMdl_psi_drift from tf import transformations -from numpy import unwrap -from numpy import * # input variables cmd_servo = 0 cmd_motor = 0 -d_f = 0 FxR = 0 # raw measurement variables @@ -52,9 +46,9 @@ x_meas = 0 y_meas = 0 -x_hist = [0]*30 -y_hist = [0]*30 -gps_times = [0]*30 +x_hist = [0]*25 +y_hist = [0]*25 +gps_times = [0]*25 poly_x = [0]*3 # polynomial coefficients for x and y position measurement (2nd order) poly_y = [0]*3 @@ -65,9 +59,9 @@ # ecu command update def ecu_callback(data): global FxR, cmd_motor, cmd_servo, running - cmd_motor = data.motor # input motor force [N] - cmd_servo = data.servo # input steering angle [rad] - FxR = cmd_motor + cmd_motor = data.motor # input motor force [N] + cmd_servo = data.servo # input steering angle [rad] + FxR = cmd_motor if not running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero running = True @@ -75,8 +69,8 @@ def ecu_callback(data): def gps_callback(data): # units: [rad] and [rad/s] global x_meas, y_meas, t0 - x_meas_pred = polyval(poly_x,data.timestamp_ros-t0) # predict new position - y_meas_pred = polyval(poly_y,data.timestamp_ros-t0) + x_meas_pred = polyval(poly_x, data.timestamp_ros-t0) # predict new position + y_meas_pred = polyval(poly_y, data.timestamp_ros-t0) if abs(x_meas_pred-data.x_m) < 0.5 and abs(y_meas_pred-data.y_m) < 0.5 or not running: # check for outlier x_meas = data.x_m # data is given in cm @@ -98,11 +92,11 @@ def imu_callback(data): # get orientation from quaternion data, and convert to roll, pitch, yaw # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) (roll_meas, pitch_meas, yaw_meas) = transformations.euler_from_quaternion(quaternion) # yaw_meas is element of [-pi,pi] - yaw = unwrap([yaw_prev,yaw_meas])[1] # get smooth yaw (from beginning) + yaw = unwrap([yaw_prev, yaw_meas])[1] # get smooth yaw (from beginning) yaw_prev = yaw # and always use raw measured yaw for unwrapping # from this point on 'yaw' will be definitely unwrapped (smooth)! if not running: @@ -113,7 +107,7 @@ def imu_callback(data): imu_times.append(data.header.stamp.to_sec()-t0) imu_times.pop(0) - + # extract angular velocity and linear acceleration data w_x = data.angular_velocity.x w_y = data.angular_velocity.y @@ -145,52 +139,50 @@ def state_estimation(): rospy.Subscriber('vel_est', Vel_est, vel_est_callback) rospy.Subscriber('ecu', ECU, ecu_callback) rospy.Subscriber('hedge_pos', hedge_pos, gps_callback) - state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size = 1) - + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + # get vehicle dimension parameters L_f = rospy.get_param("L_a") # distance from CoG to front axel L_r = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_f, L_r) + vhMdl = (L_f, L_r) # get EKF observer properties - q_std = rospy.get_param("state_estimation_dynamic/q_std") # std of process noise - psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation - gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - ang_v_std = rospy.get_param("state_estimation_dynamic/ang_v_std") # std of angular velocity measurements - est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode + psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise + v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation + gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements + est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode # set node rate - loop_rate = 25 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - t0 = rospy.get_rostime().to_sec() + loop_rate = 25 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + t0 = rospy.get_rostime().to_sec() # settings about psi estimation (use different models accordingly) psi_drift_active = True psi_drift = 0 if psi_drift_active: - z_EKF = zeros(5) - P = eye(5) # initial dynamics coveriance matrix - Q = diag([5.0,5.0,5.0,10.0,1.0])*dt + z_EKF = zeros(5) # x, y, psi, v, psi_drift + P = eye(5) # initial dynamics coveriance matrix + Q = diag([5.0, 5.0, 5.0, 10.0, 1.0])*dt else: - z_EKF = zeros(4) - P = eye(4) # initial dynamics coveriance matrix - Q = diag([5.0,5.0,5.0,5.0])*dt - - if est_mode==1: # use gps, IMU, and encoder - print("Using GPS, IMU and encoders.") - R = diag([gps_std,gps_std,psi_std,v_std])**2 - elif est_mode==2: # use IMU and encoder only - print("Using IMU and encoders.") - R = diag([psi_std,v_std])**2 - elif est_mode==3: # use gps only - print("Using GPS.") + z_EKF = zeros(4) + P = eye(4) # initial dynamics coveriance matrix + Q = diag([5.0, 5.0, 5.0, 5.0])*dt + + if est_mode == 1: # use gps, IMU, and encoder + print "Using GPS, IMU and encoders." + R = diag([gps_std, gps_std, psi_std, v_std])**2 + elif est_mode == 2: # use IMU and encoder only + print "Using IMU and encoders." + R = diag([psi_std, v_std])**2 + elif est_mode == 3: # use gps only + print "Using GPS." R = (gps_std**2)*eye(2) - elif est_mode==4: # use gps and encoder - print("Using GPS and encoders.") - R = diag([gps_std,gps_std,v_std])**2 + elif est_mode == 4: # use gps and encoder + print "Using GPS and encoders." + R = diag([gps_std, gps_std, v_std])**2 else: rospy.logerr("No estimation mode selected.") @@ -199,113 +191,92 @@ def state_estimation(): l.create_track() l.prepare_trajectory(0.06) - psi_prev = 0 # for debugging - d_f = 0 while not rospy.is_shutdown(): t = rospy.get_rostime().to_sec()-t0 # current time - # calculate new steering angle (low pass filter on steering input) + # calculate new steering angle (low pass filter on steering input to make v_y and v_x smoother) d_f = d_f + (cmd_servo-d_f)*0.25 + # ***** EXTRAPOLATE MEASUREMENTS TO CURRENT TIME ************************** # update x and y polynomial: - t_matrix = vstack((gps_times,gps_times,ones(size(gps_times)))) - t_matrix[0] = t_matrix[0]**2 - poly_x = linalg.lstsq(t_matrix.T,x_hist)[0] - poly_y = linalg.lstsq(t_matrix.T,y_hist)[0] + t_matrix = vstack((gps_times, gps_times, ones(size(gps_times)))) + t_matrix[0] = t_matrix[0]**2 + poly_x = linalg.lstsq(t_matrix.T, x_hist)[0] + poly_y = linalg.lstsq(t_matrix.T, y_hist)[0] # calculate current position from interpolated measurements - x_meas_pred = polyval(poly_x,t) - y_meas_pred = polyval(poly_y,t) - - # print "Times:" - # print gps_times - # print "x-values:" - # print x_hist - # print "ROS time: %f"%t - # print "Polynomial coefficients:" - # print poly_x - # print "tdif = %f"%(t-gps_times[-1]) - # print "Pred. x_meas: %f, real x_meas: %f"%(x_meas_pred,x_meas) + x_meas_pred = polyval(poly_x, t) + y_meas_pred = polyval(poly_y, t) # update velocity estimation polynomial: - t_matrix_vel = vstack((vel_times,ones(size(vel_times)))) - poly_vel = linalg.lstsq(t_matrix_vel.T,vel_est_hist)[0] - vel_est_pred = polyval(poly_vel,t) - # print "Times:" - # print vel_times - # print "Meas:" - # print vel_est_hist - # print "ROS time: %f"%t - # print "tdif = %f"%(t-vel_times[-1]) - # print "Pred. vel: %f, real vel_est: %f"%(vel_est_pred,vel_est) + t_matrix_vel = vstack((vel_times, ones(size(vel_times)))) + poly_vel = linalg.lstsq(t_matrix_vel.T, vel_est_hist)[0] + vel_est_pred = polyval(poly_vel, t) # update IMU polynomial: - t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) - t_matrix_imu[0] = t_matrix_imu[0]**2 - poly_psiDot = linalg.lstsq(t_matrix_imu.T,psiDot_hist)[0] - psiDot_meas_pred = polyval(poly_psiDot,t) + t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) + t_matrix_imu[0] = t_matrix_imu[0]**2 + poly_psiDot = linalg.lstsq(t_matrix_imu.T, psiDot_hist)[0] + psiDot_meas_pred = polyval(poly_psiDot, t) if psi_drift_active: - (x,y,psi,v,psi_drift) = z_EKF # note, r = EKF estimate yaw rate + (x, y, psi, v, psi_drift) = z_EKF # note, r = EKF estimate yaw rate else: - (x,y,psi,v) = z_EKF # note, r = EKF estimate yaw rate + (x, y, psi, v) = z_EKF # note, r = EKF estimate yaw rate # use Kalman values to predict state in 0.1s dt_pred = 0.0 bta = arctan(L_f/(L_f+L_r)*tan(d_f)) - x_pred = x + dt_pred*( v*cos(psi + bta) ) - y_pred = y + dt_pred*( v*sin(psi + bta) ) - psi_pred = psi + dt_pred*v/L_r*sin(bta) - v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) - v_x_pred = cos(bta)*v_pred - v_y_pred = sin(bta)*v_pred + x_pred = x + dt_pred*(v*cos(psi + bta)) + y_pred = y + dt_pred*(v*sin(psi + bta)) + psi_pred = psi + dt_pred*v/L_r*sin(bta) + v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) + v_x_pred = cos(bta)*v_pred + v_y_pred = sin(bta)*v_pred psi_dot_pred = psiDot_meas_pred # Update track position - l.set_pos(x_pred,y_pred,psi_pred,v_x_pred,v_x_pred,v_y_pred,psi_dot_pred) # v = v_x + l.set_pos(x_pred, y_pred, psi_pred, v_x_pred, v_x_pred, v_y_pred, psi_dot_pred) # v = v_x l.find_s() # and then publish position info - state_pub_pos.publish( pos_info(l.s,l.ey,l.epsi,l.v,l.s_start,l.x,l.y,l.v_x,l.v_y,l.psi,l.psiDot,l.coeffX.tolist(),l.coeffY.tolist(),l.coeffTheta.tolist(),l.coeffCurvature.tolist()) ) + state_pub_pos.publish(pos_info(l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, + l.psi, l.psiDot, l.coeffX.tolist(), l.coeffY.tolist(), + l.coeffTheta.tolist(), l.coeffCurvature.tolist())) # apply EKF # get measurement - if est_mode==1: - y = array([x_meas_pred,y_meas_pred,yaw,vel_est]) - elif est_mode==2: - y = array([yaw,vel_est]) - elif est_mode==3: - y = array([x_meas,y_meas]) - elif est_mode==4: - y = array([x_meas,y_meas,vel_est]) + if est_mode == 1: + y = array([x_meas_pred, y_meas_pred, yaw, vel_est]) + elif est_mode == 2: + y = array([yaw, vel_est]) + elif est_mode == 3: + y = array([x_meas, y_meas]) + elif est_mode == 4: + y = array([x_meas, y_meas, vel_est]) else: - print("Wrong estimation mode specified.") + print "Wrong estimation mode specified." # define input - u = array([ d_f, FxR ]) + u = array([d_f, FxR]) # build extra arguments for non-linear function - args = (u, vhMdl, dt, est_mode) + args = (u, vhMdl, dt, est_mode) # apply EKF and get each state estimate if psi_drift_active: - (z_EKF,P) = ekf(f_KinBkMdl_psi_drift, z_EKF, P, h_KinBkMdl_psi_drift, y, Q, R, args ) + (z_EKF, P) = ekf(f_KinBkMdl_psi_drift, z_EKF, P, h_KinBkMdl_psi_drift, y, Q, R, args) else: - (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) + (z_EKF, P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args) - #print("yaw = %f, psi = %f"%(yaw,psi_pred)) - if abs(psi_pred-psi_prev) > 0.2: - print("WAAAAAAAAAAAAARNING LARGE PSI DIFFERENCE!!!!!!!!!!!!!!!!!!!******************\n") - print("*****************************************************************************\n") - psi_prev = psi_pred # wait rate.sleep() if __name__ == '__main__': try: - state_estimation() + state_estimation() except rospy.ROSInterruptException: pass diff --git a/workspace/src/marvelmind_indoor_gps/CMakeLists.txt b/workspace/src/marvelmind_indoor_gps/CMakeLists.txt deleted file mode 100644 index 9329559e..00000000 --- a/workspace/src/marvelmind_indoor_gps/CMakeLists.txt +++ /dev/null @@ -1,187 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(marvelmind_indoor_gps) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - std_msgs - rospy -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES marvelmind_indoor_gps -# CATKIN_DEPENDS rospy -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(marvelmind_indoor_gps -# src/${PROJECT_NAME}/marvelmind_indoor_gps.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(marvelmind_indoor_gps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(marvelmind_indoor_gps_node src/marvelmind_indoor_gps_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(marvelmind_indoor_gps_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(marvelmind_indoor_gps_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS marvelmind_indoor_gps marvelmind_indoor_gps_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_marvelmind_indoor_gps.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch b/workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch deleted file mode 100644 index 5991fae4..00000000 --- a/workspace/src/marvelmind_indoor_gps/launch/indoor_gps.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/workspace/src/marvelmind_indoor_gps/package.xml b/workspace/src/marvelmind_indoor_gps/package.xml deleted file mode 100644 index 8dd95296..00000000 --- a/workspace/src/marvelmind_indoor_gps/package.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - marvelmind_indoor_gps - 0.0.0 - The marvelmind_indoor_gps package - odroid - MIT - catkin - rospy - rospy - - - - diff --git a/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py b/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py deleted file mode 100755 index a7d47881..00000000 --- a/workspace/src/marvelmind_indoor_gps/src/indoor_gps.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python - -import rospy -import sys -from time import sleep -import serial -import struct -import time -from numpy import pi -from geometry_msgs.msg import Vector3 -import crcmod -import traceback - -def indoor_gps_init(serial_device, baudrate): - # open port - try: - ser = serial.Serial(serial_device, baudrate, timeout=3 ) - rospy.loginfo("opened port!") - except serial.serialutil.SerialException: - print 'Can not open serial port(%s)'%(serial_device) - traceback.print_exc() - return None - - # return serial port - ser.flushInput() - return ser - -def indoor_gps_data_acq(): - # start node - rospy.init_node('indoor_gps', anonymous=True) - pub = rospy.Publisher('indoor_gps', Vector3, queue_size = 1) - - # open port - port = rospy.get_param("indoor_gps/port") - baud = rospy.get_param("indoor_gps/baud") - ser = indoor_gps_init(port, baud) - - pktSize = 23 - - while not rospy.is_shutdown(): - ser = serial.Serial(port, baud, timeout=3 ) - ser.flushInput() - len_buf = pktSize*3 - 1 - buf = ser.read(len_buf) - buf = '' - - while buf is not None: - buf += ser.read(len_buf) - pktHdrOffset = buf.find('\xff\x47\x01\x00') - - while pktHdrOffset == -1: - ser.flushInput() - buf = ser.read(len_buf) - pktHdrOffset = buf.find('\xff\x47\x01\x00') - - if len_buf-pktHdrOffset-5>=18: - usnTimestamp, usnX, usnY, usnCRC16 = struct.unpack_from ( ' Date: Wed, 19 Oct 2016 18:07:08 -0700 Subject: [PATCH 069/183] Added headers to custom messages and adapted code accordingly. Restructured low level control so that there are no more global variables. --- workspace/src/barc/msg/ECU.msg | 1 + workspace/src/barc/msg/Vel_est.msg | 4 +- workspace/src/barc/msg/pos_info.msg | 1 + workspace/src/barc/src/LMPC_node.jl | 3 +- workspace/src/barc/src/barc_simulator_dyn.jl | 11 ++- .../src/barc/src/controller_low_level.py | 72 +++++++++---------- .../src/barc/src/state_estimation_DynBkMdl.py | 14 ++-- .../ros_marvelmind_package/msg/hedge_pos.msg | 2 +- .../src/hedge_rcv_bin.cpp | 5 +- 9 files changed, 60 insertions(+), 53 deletions(-) diff --git a/workspace/src/barc/msg/ECU.msg b/workspace/src/barc/msg/ECU.msg index 2bffecbf..a37117f5 100644 --- a/workspace/src/barc/msg/ECU.msg +++ b/workspace/src/barc/msg/ECU.msg @@ -6,5 +6,6 @@ # # For modeling and state estimation, motors units are [N], and servo units are [rad] # For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle +Header header float32 motor float32 servo diff --git a/workspace/src/barc/msg/Vel_est.msg b/workspace/src/barc/msg/Vel_est.msg index 10d26b83..1ade9b44 100644 --- a/workspace/src/barc/msg/Vel_est.msg +++ b/workspace/src/barc/msg/Vel_est.msg @@ -1,2 +1,2 @@ -float32 vel_est -time stamp \ No newline at end of file +Header header +float32 vel_est \ No newline at end of file diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 7fbcb2a7..2063d852 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -1,5 +1,6 @@ # This is a message to hold data with position and trajectory information +Header header float64 s float64 ey float64 epsi diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 6ca9ffd7..a08af715 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -60,7 +60,7 @@ function main() coeffX = zeros(9) # buffer for coeffX (only logging) coeffY = zeros(9) # buffer for coeffY (only logging) s_start_update = [0.0] # buffer for s_start - cmd = ECU(0.0,0.0) # command type + cmd = ECU() # command type coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables @@ -127,6 +127,7 @@ function main() # ============================= PUBLISH COMMANDS ============================= # this is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received # the state is predicted by 0.1s + cmd.header.stamp = get_rostime() cmd.motor = convert(Float32,mpcSol.a_x) cmd.servo = convert(Float32,mpcSol.d_f) publish(pub, cmd) diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index f676c0dd..04de3bbc 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -19,6 +19,7 @@ using RobotOS @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos +@rosimport std_msgs.msg: Header rostypegen() using barc.msg using data_service.msg @@ -106,11 +107,15 @@ function main() imu_data = Imu() vel_est = Vel_est() t0 = time() + gps_data = hedge_pos() + t = 0.0 sim_gps_interrupt = 0 # counter if gps interruption is simulated vel_pos = zeros(2) # position when velocity was updated last time vel_dist_update = 2*pi*0.036/2 # distance to travel until velocity is updated (half wheel rotation) + + gps_header = Header() while ! is_shutdown() t = time() @@ -141,7 +146,7 @@ function main() vel_est.vel_est = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) vel_pos = z_current[i,1:2][:] end - vel_est.stamp = t_ros + vel_est.header.stamp = t_ros publish(pub_vel, vel_est) end @@ -160,7 +165,9 @@ function main() gps_meas.i += 1 gps_meas.t[gps_meas.i] = t gps_meas.z[gps_meas.i,:] = [x y] - gps_data = hedge_pos(0,convert(Float64,get_rostime()),x,y,0,0) + gps_data.header.stamp = get_rostime() + gps_data.x_m = x + gps_data.y_m = y publish(pub_gps, gps_data) end sim_gps_interrupt -= 1 diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 6d4bc924..9d7345cd 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -18,59 +18,55 @@ # Subscribes: steering and motor commands on 'ecu' # Publishes: combined ecu commands as 'ecu_pwm' -from rospy import init_node, Subscriber, Publisher, get_param +from rospy import init_node, Subscriber, Publisher, get_param, get_rostime from rospy import Rate, is_shutdown, ROSInterruptException, spin, on_shutdown from barc.msg import ECU from numpy import pi import rospy -motor_pwm = 90 -servo_pwm = 90 -str_ang_max = 35 -str_ang_min = -35 - -def pwm_converter_callback(msg): - global motor_pwm, servo_pwm - global str_ang_max, str_ang_min - - # translate from SI units in vehicle model - # to pwm angle units (i.e. to send command signal to actuators) - - # convert desired steering angle to degrees, saturate based on input limits - servo_pwm = 91.365 + 105.6*float(msg.servo) - - # compute motor command - FxR = float(msg.motor) - if FxR == 0: - motor_pwm = 90.0 - elif FxR > 0: - motor_pwm = 94.14 + 2.7678*FxR - else: # motor break / slow down - motor_pwm = 93.5 + 46.73*FxR - update_arduino() - -def neutralize(): - global motor_pwm +class low_level_control(object): motor_pwm = 90 servo_pwm = 90 - update_arduino() - -def update_arduino(): - global motor_pwm, servo_pwm, ecu_pub - ecu_cmd = ECU(motor_pwm, servo_pwm) - ecu_pub.publish(ecu_cmd) + str_ang_max = 35 + str_ang_min = -35 + ecu_pub = 0 + FxR = 0 + ecu_cmd = ECU() + def pwm_converter_callback(self, msg): + # translate from SI units in vehicle model + # to pwm angle units (i.e. to send command signal to actuators) + # convert desired steering angle to degrees, saturate based on input limits + self.servo_pwm = 91.365 + 105.6*float(msg.servo) + # compute motor command + self.FxR = float(msg.motor) + if self.FxR == 0: + self.motor_pwm = 90.0 + elif self.FxR > 0: + self.motor_pwm = 94.14 + 2.7678*self.FxR + else: # motor break / slow down + self.motor_pwm = 93.5 + 46.73*self.FxR + self.update_arduino() + def neutralize(self): + self.motor_pwm = 90 + self.servo_pwm = 90 + self.update_arduino() + def update_arduino(self): + self.ecu_cmd.header.stamp = get_rostime() + self.ecu_cmd.motor = self.motor_pwm + self.ecu_cmd.servo = self.servo_pwm + self.ecu_pub.publish(self.ecu_cmd) def arduino_interface(): - global ecu_pub # launch node, subscribe to motorPWM and servoPWM, publish ecu init_node('arduino_interface') + llc = low_level_control() - Subscriber('ecu', ECU, pwm_converter_callback, queue_size = 1) - ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1) + Subscriber('ecu', ECU, llc.pwm_converter_callback, queue_size = 1) + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1) # Set motor to neutral on shutdown - on_shutdown(neutralize) + on_shutdown(llc.neutralize) # process callbacks and keep alive spin() diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 1f12a762..3d89fcac 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -18,6 +18,7 @@ from barc.msg import ECU, pos_info, Vel_est from sensor_msgs.msg import Imu from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, size, sign from numpy import unwrap, vstack, ones, linalg, polyval from observers import ekf @@ -69,8 +70,8 @@ def ecu_callback(data): def gps_callback(data): # units: [rad] and [rad/s] global x_meas, y_meas, t0 - x_meas_pred = polyval(poly_x, data.timestamp_ros-t0) # predict new position - y_meas_pred = polyval(poly_y, data.timestamp_ros-t0) + x_meas_pred = polyval(poly_x, data.header.stamp.to_sec()-t0) # predict new position + y_meas_pred = polyval(poly_y, data.header.stamp.to_sec()-t0) if abs(x_meas_pred-data.x_m) < 0.5 and abs(y_meas_pred-data.y_m) < 0.5 or not running: # check for outlier x_meas = data.x_m # data is given in cm @@ -78,7 +79,7 @@ def gps_callback(data): x_hist.append(x_meas) y_hist.append(y_meas) - gps_times.append(data.timestamp_ros-t0) + gps_times.append(data.header.stamp.to_sec()-t0) x_hist.pop(0) y_hist.pop(0) gps_times.pop(0) @@ -125,7 +126,7 @@ def vel_est_callback(data): vel_est = data.vel_est vel_est_hist.append(vel_est) vel_est_hist.pop(0) - vel_times.append(data.stamp.to_sec()-t0) + vel_times.append(data.header.stamp.to_sec()-t0) vel_times.pop(0) # state estimation node @@ -194,7 +195,8 @@ def state_estimation(): d_f = 0 while not rospy.is_shutdown(): - t = rospy.get_rostime().to_sec()-t0 # current time + ros_t = rospy.get_rostime() + t = ros_t.to_sec()-t0 # current time # calculate new steering angle (low pass filter on steering input to make v_y and v_x smoother) d_f = d_f + (cmd_servo-d_f)*0.25 @@ -243,7 +245,7 @@ def state_estimation(): l.find_s() # and then publish position info - state_pub_pos.publish(pos_info(l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, l.psi, l.psiDot, l.coeffX.tolist(), l.coeffY.tolist(), l.coeffTheta.tolist(), l.coeffCurvature.tolist())) diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg index fbfd84ac..1d141d04 100644 --- a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg @@ -1,5 +1,5 @@ +Header header int64 timestamp_ms -float64 timestamp_ros float64 x_m float64 y_m float64 z_m diff --git a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp index cc86ef94..ceebfde7 100644 --- a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp +++ b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp @@ -57,7 +57,7 @@ static bool hedgeReceiveCheck(void) hedge_pos_msg.timestamp_ms= position.timestamp*15.625;// alpha-cycles ==> msec } - hedge_pos_msg.timestamp_ros= ros::Time::now().toSec(); // added by mb for custom hedge_pos msg + hedge_pos_msg.header.stamp= ros::Time::now(); // added by mb for custom hedge_pos msg hedge_pos_msg.x_m= position.x/100.0; hedge_pos_msg.y_m= position.y/100.0; hedge_pos_msg.z_m= position.z/100.0; @@ -88,7 +88,6 @@ int main(int argc, char **argv) // default values for position message hedge_pos_msg.timestamp_ms = 0; - hedge_pos_msg.timestamp_ros = 0; // added by mb hedge_pos_msg.x_m = 0.0; hedge_pos_msg.y_m = 0.0; hedge_pos_msg.z_m = 0.0; @@ -107,7 +106,7 @@ int main(int argc, char **argv) ROS_INFO("%d, %d, %.3f, X=%.3f Y= %.3f Z=%.3f flags=%d", (int) hedge_pos_msg.timestamp_ms, (int) (hedge_pos_msg.timestamp_ms - hedge_timestamp_prev), - (float) hedge_pos_msg.timestamp_ros, + (float) hedge_pos_msg.header.stamp.toSec(), (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, (int) hedge_pos_msg.flags); hedge_pos_publisher.publish(hedge_pos_msg); From 3c98c68cc323c6e82a5de094210aced99dfed72e Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 20 Oct 2016 10:18:23 -0700 Subject: [PATCH 070/183] Switched to ROS time as global clock. Messages are logged with published and received time. --- eval_sim/eval_data.jl | 11 ++-- workspace/src/barc/src/barc_record.jl | 54 +++++++++-------- workspace/src/barc/src/barc_simulator_dyn.jl | 62 +++++++++++--------- 3 files changed, 69 insertions(+), 58 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 2f513cea..f43b775c 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -16,9 +16,10 @@ using HDF5, JLD, ProfileView include("../workspace/src/barc/src/barc_lib/classes.jl") type Measurements{T} - i::Int64 # measurement counter - t::Array{Float64} # time data - z::Array{T} # measurement values + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values end const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" # data from MPC @@ -78,8 +79,8 @@ function eval_sim(code::AbstractString) figure() title("Comparison of v") - plot(z.t,z.z[:,3:4],pos_info.t,pos_info.z[:,8:9],"-*",vel_est.t,vel_est.z) - legend(["real_xDot","real_yDot","est_xDot","est_yDot","v_x_meas"]) + plot(z.t,z.z[:,3:4],z.t,sqrt(z.z[:,3].^2+z.z[:,4].^2),pos_info.t,pos_info.z[:,8:9],"-*",pos_info.t,sqrt(pos_info.z[:,8].^2+pos_info.z[:,9].^2),"-*",vel_est.t,vel_est.z) + legend(["real_xDot","real_yDot","real_v","est_xDot","est_yDot","est_v","v_x_meas"]) grid() figure() diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 9838c033..209eab3c 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -26,15 +26,17 @@ using JLD # This type contains measurement data (time, values and a counter) type Measurements{T} - i::Int64 # measurement counter - t::Array{Float64} # time data - z::Array{T} # measurement values + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values end # This function cleans the zeros from the type above once the simulation is finished function clean_up(m::Measurements) - m.t = m.t[1:m.i-1] - m.z = m.z[1:m.i-1,:] + m.t = m.t[1:m.i-1] + m.t_msg = m.t_msg[1:m.i-1] + m.z = m.z[1:m.i-1,:] end function Quat2Euler(q::Array{Float64}) @@ -46,56 +48,56 @@ function Quat2Euler(q::Array{Float64}) end function ECU_callback(msg::ECU,cmd_log::Measurements) - cmd_log.i += 1 - cmd_log.t[cmd_log.i] = time() + cmd_log.t[cmd_log.i] = to_sec(get_rostime()) + cmd_log.t_msg[cmd_log.i] = to_sec(msg.header.stamp) cmd_log.z[cmd_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) + cmd_log.i += 1 nothing end function IMU_callback(msg::Imu,imu_meas::Measurements) - imu_meas.i += 1 - imu_meas.t[imu_meas.i] = time() + imu_meas.t[imu_meas.i] = to_sec(get_rostime()) + imu_meas.t_msg[imu_meas.i] = to_sec(msg.header.stamp) imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z; Quat2Euler([msg.orientation.w;msg.orientation.x;msg.orientation.y;msg.orientation.z]); msg.linear_acceleration.x;msg.linear_acceleration.y;msg.linear_acceleration.z]::Array{Float64} + imu_meas.i += 1 nothing end function GPS_callback(msg::hedge_pos,gps_meas::Measurements) - gps_meas.i += 1 - gps_meas.t[gps_meas.i] = time() + gps_meas.t[gps_meas.i] = to_sec(get_rostime()) + gps_meas.t_msg[gps_meas.i] = to_sec(msg.header.stamp) gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] + gps_meas.i += 1 nothing end function pos_info_callback(msg::pos_info,pos_info_log::Measurements) - pos_info_log.i += 1 - pos_info_log.t[pos_info_log.i] = time() + pos_info_log.t[pos_info_log.i] = to_sec(get_rostime()) + pos_info_log.t_msg[pos_info_log.i] = to_sec(msg.header.stamp) pos_info_log.z[pos_info_log.i,:] = [msg.s;msg.ey;msg.epsi;msg.v;msg.s_start;msg.x;msg.y;msg.v_x;msg.v_y;msg.psi;msg.psiDot] + pos_info_log.i += 1 nothing end function vel_est_callback(msg::Vel_est,vel_est_log::Measurements) - vel_est_log.i += 1 - vel_est_log.t[vel_est_log.i] = time() + vel_est_log.t[vel_est_log.i] = to_sec(get_rostime()) + vel_est_log.t_msg[vel_est_log.i] = to_sec(msg.header.stamp) vel_est_log.z[vel_est_log.i] = convert(Float64,msg.vel_est) + vel_est_log.i += 1 nothing end function main() buffersize = 60000 - gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,9)) - cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - vel_est_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize)) - pos_info_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,11)) - - gps_meas.t[1] = time() - imu_meas.t[1] = time() - cmd_log.t[1] = time() - pos_info_log.t[1] = time() - vel_est_log.t[1] = time() + gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) + cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,11)) + # initiate node, set up publisher / subscriber topics init_node("barc_record") s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 04de3bbc..3a19f9df 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -36,36 +36,34 @@ include("barc_lib/simModel.jl") type Measurements{T} i::Int64 # measurement counter t::Array{Float64} # time data + t_msg::Array{Float64} z::Array{T} # measurement values end # This function cleans the zeros from the type above once the simulation is finished function clean_up(m::Measurements) - m.t = m.t[1:m.i-1] - m.z = m.z[1:m.i-1,:] + m.t = m.t[1:m.i-1] + m.t_msg = m.t_msg[1:m.i-1] + m.z = m.z[1:m.i-1,:] end function ECU_callback(msg::ECU,u_current::Array{Float64},cmd_log::Measurements) u_current[:] = convert(Array{Float64,1},[msg.motor, msg.servo]) + cmd_log.t_msg[cmd_log.i] = to_sec(get_rostime()) + cmd_log.t[cmd_log.i] = to_sec(get_rostime()) + cmd_log.z[cmd_log.i,:] = u_current cmd_log.i += 1 - cmd_log.t[cmd_log.i] = time() - cmd_log.z[cmd_log.i,:] = u_current end function main() u_current = zeros(Float64,2) # msg ECU is Float32 ! buffersize = 60000 - gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,8)) - slip_a = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) - - z_real.t[1] = time() - slip_a.t[1] = time() - imu_meas.t[1] = time() - cmd_log.t[1] = time() + gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + z_real = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,8)) + slip_a = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) # initiate node, set up publisher / subscriber topics init_node("barc_sim") @@ -78,6 +76,7 @@ function main() z_current = zeros(60000,8) z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0 0.0] slip_ang = zeros(60000,2) + dt = 0.01 loop_rate = Rate(1/dt) @@ -100,29 +99,35 @@ function main() modelParams.l_A = 0.125 modelParams.l_B = 0.125 - modelParams.m = 1.98 + modelParams.m = 1.98 modelParams.I_z = 0.24 println("Publishing sensor information. Simulator running.") imu_data = Imu() vel_est = Vel_est() - t0 = time() + t0 = to_sec(get_rostime()) gps_data = hedge_pos() - t = 0.0 + + z_real.t_msg[1] = t0 + slip_a.t_msg[1] = t0 + z_real.t[1] = t0 + slip_a.t[1] = t0 + t = 0.0 sim_gps_interrupt = 0 # counter if gps interruption is simulated - vel_pos = zeros(2) # position when velocity was updated last time vel_dist_update = 2*pi*0.036/2 # distance to travel until velocity is updated (half wheel rotation) gps_header = Header() while ! is_shutdown() - - t = time() - t_ros = get_rostime() # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:],u_current', dt, modelParams) + t_ros = get_rostime() + t = to_sec(t_ros) + + z_real.t_msg[i] = t z_real.t[i] = t + slip_a.t_msg[i] = t slip_a.t[i] = t # IMU measurements @@ -130,9 +135,10 @@ function main() imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) yaw = z_current[i,5] + randn()*0.02 + imu_drift psiDot = z_current[i,6] + 0.01*randn() - imu_meas.i += 1 + imu_meas.t_msg[imu_meas.i] = t imu_meas.t[imu_meas.i] = t imu_meas.z[imu_meas.i,:] = [yaw psiDot] + imu_meas.i += 1 imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros @@ -141,10 +147,11 @@ function main() end # Velocity measurements + dist_traveled += norm(diff(z_current[i-1:i,1:2])) if i%5 == 0 # 20 Hz - if norm(z_current[i,1:2][:]-vel_pos) > vel_dist_update # only update if a magnet has passed the sensor - vel_est.vel_est = convert(Float32,norm(z_current[i,3:4])+0.01*randn()) - vel_pos = z_current[i,1:2][:] + if dist_traveled >= vel_dist_update # only update if a magnet has passed the sensor + dist_traveled = 0 + vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) end vel_est.header.stamp = t_ros publish(pub_vel, vel_est) @@ -162,9 +169,10 @@ function main() sim_gps_interrupt = 10 # simulate gps-interrupt (10 steps at 25 Hz is 0.4 seconds) end if sim_gps_interrupt < 0 - gps_meas.i += 1 + gps_meas.t_msg[gps_meas.i] = t gps_meas.t[gps_meas.i] = t gps_meas.z[gps_meas.i,:] = [x y] + gps_meas.i += 1 gps_data.header.stamp = get_rostime() gps_data.x_m = x gps_data.y_m = y @@ -191,7 +199,7 @@ function main() log_path = "$(homedir())/simulations/output-SIM-$(run_id[1:4]).jld" save(log_path,"gps_meas",gps_meas,"z",z_real,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") - #writedlm(log_path,z_current[1:i-1,:]) + end if ! isinteractive() From 2fe785712c8d39049d5eb9a0ae6d54fc9f3bb06d Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 20 Oct 2016 11:24:28 -0700 Subject: [PATCH 071/183] Cleaned code, added open loop julia control file. --- eval_sim/eval_data.jl | 15 +- workspace/src/barc/launch/barc_sim.launch | 15 +- .../src/barc/launch/open_loop_remote.launch | 53 ++++++ workspace/src/barc/src/LMPC_node_profiler.jl | 179 ------------------ workspace/src/barc/src/barc_record.jl | 13 +- workspace/src/barc/src/barc_simulator_dyn.jl | 3 +- workspace/src/barc/src/controller_mpc.jl | 109 ----------- workspace/src/barc/src/open_loop.jl | 61 ++++++ 8 files changed, 146 insertions(+), 302 deletions(-) create mode 100644 workspace/src/barc/launch/open_loop_remote.launch delete mode 100644 workspace/src/barc/src/LMPC_node_profiler.jl delete mode 100755 workspace/src/barc/src/controller_mpc.jl create mode 100755 workspace/src/barc/src/open_loop.jl diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index f43b775c..7d8ae9f1 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -22,10 +22,6 @@ type Measurements{T} z::Array{T} # measurement values end -const log_path_LMPC = "$(homedir())/simulations/output_LMPC.jld" # data from MPC -const log_path_sim = "$(homedir())/simulations/output.jld" # data from barc_simulation -const log_path_record = "$(homedir())/simulations/record-2016-10-18-16-53-56.jld" # data from barc_record - # THIS FUNCTION EVALUATES DATA THAT WAS LOGGED BY THE SIMULATOR (INCLUDES "REAL" SIMULATION DATA) # *********************************************************************************************** @@ -99,6 +95,7 @@ function eval_run(code::AbstractString) imu_meas = d_rec["imu_meas"] gps_meas = d_rec["gps_meas"] cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] vel_est = d_rec["vel_est"] pos_info = d_rec["pos_info"] @@ -122,6 +119,16 @@ function eval_run(code::AbstractString) ylabel("Position [m]") legend(["x_meas","y_meas","x_est","y_est"]) + figure() + ax2=subplot(211) + plot(cmd_log.t,cmd_log.z,"-*",cmd_log.t_msg,cmd_log.z,"-x") + grid("on") + xlabel("t [s]") + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t,cmd_pwm_log.z,"-*") + grid("on") + xlabel("t [s]") + figure() title("Comparison of psi") plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*") diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 6c7a885c..7f095805 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -7,7 +7,14 @@ - + + + + + + + + @@ -26,10 +33,4 @@ - - - - - - diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch new file mode 100644 index 00000000..4bdd8580 --- /dev/null +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/src/LMPC_node_profiler.jl b/workspace/src/barc/src/LMPC_node_profiler.jl deleted file mode 100644 index 66506e64..00000000 --- a/workspace/src/barc/src/LMPC_node_profiler.jl +++ /dev/null @@ -1,179 +0,0 @@ -#!/usr/bin/env julia - -using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging -@rosimport data_service.msg: TimeData -@rosimport geometry_msgs.msg: Vector3 -rostypegen() -using barc.msg -using data_service.msg -using geometry_msgs.msg -using JuMP -using Ipopt -using JLD - -include("helper/classes.jl") -include("helper/coeffConstraintCost.jl") -include("helper/solveMpcProblem.jl") -include("helper/computeCostLap.jl") -include("helper/functions.jl") - -function main() - println("now starting the node") - # initiate node, set up publisher / subscriber topics - #init_node("mpc_traj") - #loop_rate = Rate(10) - - buffersize = 700 - - # Create data to be saved - save_oldTraj = zeros(buffersize,4,2,4) # max. 4 laps - - # Define and initialize variables - oldTraj = OldTrajectory() - posInfo = PosInfo() - mpcCoeff = MpcCoeff() - lapStatus = LapStatus(1,1) - mpcSol = MpcSol() - trackCoeff = TrackCoeff() # info about track (at current position, approximated) - modelParams = ModelParams() - mpcParams = MpcParams() - - z_est = zeros(4) - coeffCurvature_update = zeros(5) - s_start_update = [0.0] - #cmd::ECU - cmd = ECU(0.0,0.0) - #mdl::JuMP.Model - - #pub = Publisher("ecu", ECU, queue_size=10) - #pub2 = Publisher("logging", Logging, queue_size=10) - # The subscriber passes 3 arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - #s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,),queue_size=10) - - InitializeParameters(mpcParams,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) - println("Finished initialization.") - # Lap parameters - switchLap = false # initialize lap lap trigger - s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] - - # buffer in current lap - zCurr = zeros(10000,4) # contains state information in current Lap (max. 10'000 steps) - uCurr = zeros(10000,2) # contains input information - - zCurr_export = zeros(buffersize,4) - uCurr_export = zeros(buffersize,2) - - # DEFINE MODEL *************************************************************************** - # **************************************************************************************** - println("Building model...") - - z_Init = zeros(4) - - mdl = MpcModel() - InitializeModel(mdl,mpcParams,modelParams,trackCoeff,z_Init) - - - # Initial solve: - println("Initial solve...") - solve(mdl.mdl) - solve(mdl.mdl) - println("Finished.") - - z_sim = zeros(4) - for j=1:100 - z_sim[1] += 0.05 - z_est = z_sim - if z_est[1] > 0 # check if data has been received (s > 0) - println("Received data: z_est = $z_est") - println("curvature = $coeffCurvature_update") - println("s_start = $s_start_update") - - # ============================= Initialize iteration parameters ============================= - lapStatus.currentIt += 1 # count iteration - - i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr[i,:] = z_est # update state information - posInfo.s = z_est[1] # update position info - posInfo.s_start = s_start_update[1] - trackCoeff.coeffCurvature = coeffCurvature_update - - - # ======================================= Lap trigger ======================================= - # This part takes pretty long (about 0.6 seconds on my Mac) and should be faster! - if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... - # ... then select and save data - z_sim[1] = 0 - println("Saving data") - tic() - saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,buffersize,dt) - save_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] - zCurr[1,:] = zCurr[i,:] # reset counter to 1 and set current state - uCurr[1,:] = uCurr[i+1,:] # ... and input - i = 1 - lapStatus.currentLap += 1 # start next lap - lapStatus.currentIt = 1 # reset current iteration - switchLap = false - - tt = toq() - println("Saved data, t = $tt") - println("======================================== NEXT LAP ========================================") - println("cost: $(oldTraj.oldCost)") - println("oldTraj.oldTraj[:,1,1]:") - println(oldTraj.oldTraj[:,1,1]) - println("oldTraj.oldTraj[:,1,2]:") - println(oldTraj.oldTraj[:,1,2]) - elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger - switchLap = true - end - - - # ======================================= Calculate input ======================================= - println("======================================== NEW ITERATION # $i ========================================") - println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") - println("State Nr. $i = $z_est") - println("Coeff Curvature = $(trackCoeff.coeffCurvature)") - println("posInfo = $posInfo") - println("s = $(posInfo.s)") - println("s_start = $(posInfo.s_start)") - println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") - - # Find coefficients for cost and constraints - tic() - if lapStatus.currentLap > 1 - coeffConstraintCost(oldTraj,lapStatus,mpcCoeff,posInfo,mpcParams) - end - tt = toq() - println("Finished coefficients, t = $tt s") - #println("Found coefficients: mpcCoeff = $mpcCoeff") - # Solve the MPC problem - tic() - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uCurr[i,:]') - tt = toq() - # Write in current input information - uCurr[i+1,:] = [mpcSol.a_x mpcSol.d_f] - #println("trackCoeff = $trackCoeff") - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") - # ... and publish data - #cmd = ECU(mpcSol.a_x, mpcSol.d_f) - #publish(pub, cmd) - - zCurr[i,1] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) - println("\n") - - else - println("No estimation data received!") - end - #rossleep(loop_rate) - end - # Save simulation data to file - - log_path = "$(homedir())/simulations/output_LMPC.jld" - save(log_path,"oldTraj",save_oldTraj) - println("Exiting LMPC node. Saved data.") - -end - -if ! isinteractive() - main() -end diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 209eab3c..8dcde44a 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -55,6 +55,14 @@ function ECU_callback(msg::ECU,cmd_log::Measurements) nothing end +function ECU_PWM_callback(msg::ECU,cmd_pwm_log::Measurements) + cmd_pwm_log.t[cmd_pwm_log.i] = to_sec(get_rostime()) + cmd_pwm_log.t_msg[cmd_pwm_log.i] = to_sec(msg.header.stamp) + cmd_pwm_log.z[cmd_pwm_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) + cmd_pwm_log.i += 1 + nothing +end + function IMU_callback(msg::Imu,imu_meas::Measurements) imu_meas.t[imu_meas.i] = to_sec(get_rostime()) imu_meas.t_msg[imu_meas.i] = to_sec(msg.header.stamp) @@ -95,6 +103,7 @@ function main() gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,11)) @@ -102,6 +111,7 @@ function main() init_node("barc_record") s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} + s3 = Subscriber("ecu_pwm", ECU, ECU_PWM_callback, (cmd_pwm_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} @@ -115,13 +125,14 @@ function main() clean_up(gps_meas) clean_up(imu_meas) clean_up(cmd_log) + clean_up(cmd_pwm_log) clean_up(pos_info_log) clean_up(vel_est_log) # Save simulation data to file #log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" log_path = "$(homedir())/simulations/output-record-$(run_id[1:4]).jld" - save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"pos_info",pos_info_log,"vel_est",vel_est_log) + save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) println("Exiting node... Saving recorded data to $log_path.") end diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 3a19f9df..7b771c09 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -76,7 +76,6 @@ function main() z_current = zeros(60000,8) z_current[1,:] = [0.1 0.0 0.0 0.0 0.0 0.0 0.0 0.0] slip_ang = zeros(60000,2) - dt = 0.01 loop_rate = Rate(1/dt) @@ -199,7 +198,7 @@ function main() log_path = "$(homedir())/simulations/output-SIM-$(run_id[1:4]).jld" save(log_path,"gps_meas",gps_meas,"z",z_real,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") - + end if ! isinteractive() diff --git a/workspace/src/barc/src/controller_mpc.jl b/workspace/src/barc/src/controller_mpc.jl deleted file mode 100755 index 5ad0b972..00000000 --- a/workspace/src/barc/src/controller_mpc.jl +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env julia - -#= - Licensing Information: You are free to use or extend these projects for - education or reserach purposes provided that (1) you retain this notice - and (2) you provide clear attribution to UC Berkeley, including a link - to http://barc-project.com - - Attibution Information: The barc project ROS code-base was developed - at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales - (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed - by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was - based on an open source project by Bruce Wootton -=# - -using RobotOS -@rosimport barc.msg: ECU, Encoder, Ultrasound, Z_KinBkMdl -@rosimport data_service.msg: TimeData -@rosimport geometry_msgs.msg: Vector3 -rostypegen() -using barc.msg -using data_service.msg -using geometry_msgs.msg -using JuMP -using Ipopt - -# define model parameters -L_a = 0.125 # distance from CoG to front axel -L_b = 0.125 # distance from CoG to rear axel -dt = 0.1 # time step of system -a_max = 1 # maximum acceleration - -# preview horizon -N = 5 - -# define targets [generic values] -x_ref = 10 -y_ref = 0 - -# define decision variables -# states: position (x,y), yaw angle, and velocity -# inputs: acceleration, steering angle -println("Creating kinematic bicycle model ....") -mdl = Model(solver = IpoptSolver(print_level=3)) -@defVar( mdl, x[1:(N+1)] ) -@defVar( mdl, y[1:(N+1)] ) -@defVar( mdl, psi[1:(N+1)] ) -@defVar( mdl, v[1:(N+1)] ) -@defVar( mdl, a[1:N] ) -@defVar( mdl, d_f[1:N] ) - -# define objective function -@setNLObjective(mdl, Min, (x[N+1] - x_ref)^2 + (y[N+1] - y_ref)^2 ) - -# define constraints -# define system dynamics -# Reference: R.Rajamani, Vehicle Dynamics and Control, set. Mechanical Engineering Series, -# Spring, 2011, page 26 -@defNLParam(mdl, x0 == 0); @addNLConstraint(mdl, x[1] == x0); -@defNLParam(mdl, y0 == 0); @addNLConstraint(mdl, y[1] == y0); -@defNLParam(mdl, psi0 == 0); @addNLConstraint(mdl, psi[1] == psi0 ); -@defNLParam(mdl, v0 == 0); @addNLConstraint(mdl, v[1] == v0); -@defNLExpr(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan(d_f[i]) ) ) -for i in 1:N - @addNLConstraint(mdl, x[i+1] == x[i] + dt*(v[i]*cos( psi[i] + bta[i] )) ) - @addNLConstraint(mdl, y[i+1] == y[i] + dt*(v[i]*sin( psi[i] + bta[i] )) ) - @addNLConstraint(mdl, psi[i+1] == psi[i] + dt*(v[i]/L_b * sin(bta[i])) ) - @addNLConstraint(mdl, v[i+1] == v[i] + dt*(a[i]) ) - @addConstraint(mdl, 0 <= a[i] <= a_max ) -end - -# status update -println("initial solve ...") -solve(mdl) -println("finished initial solve!") - -function SE_callback(msg::Z_KinBkMdl) - # update mpc initial condition - setValue(x0, msg.x) - setValue(y0, msg.y) - setValue(psi0, msg.psi) - setValue(v0, msg.v) -end - -function main() - # initiate node, set up publisher / subscriber topics - init_node("mpc") - pub = Publisher("ecu", ECU, queue_size=10) - sub = Subscriber("state_estimate", Z_KinBkMdl, SE_callback, queue_size=10) - loop_rate = Rate(10) - - while ! is_shutdown() - # run mpc, publish command - solve(mdl) - - # get optimal solutions - a_opt = getValue(a[1]) - d_f_opt = getValue(d_f[1]) - cmd = ECU(a_opt, d_f_opt) - - # publish commands - publish(pub, cmd) - rossleep(loop_rate) - end -end - -if ! isinteractive() - main() -end diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl new file mode 100755 index 00000000..08f03e09 --- /dev/null +++ b/workspace/src/barc/src/open_loop.jl @@ -0,0 +1,61 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU +rostypegen() +using barc.msg + + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.t_msg = m.t_msg[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + + +function main() + # Initialize ROS node and topics + init_node("mpc_traj") + loop_rate = Rate(10) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + t0_ros = get_rostime() + t0 = to_sec(t0_ros) + cmd = ECU() # command type + cmd.motor = 0 + cmd.servo = 0 + cmd.header.stamp = get_rostime() + println("Starting open loop control.") + # Start node + while ! is_shutdown() + t = to_sec(get_rostime())-t0 + if t <= 3 + cmd.motor = 0 + cmd.servo = 0 + elseif t <= 5 + cmd.motor = 1 + cmd.servo = 0 + else + cmd.motor = 0 + cmd.servo = 0 + end + + cmd.header.stamp = get_rostime() + publish(pub, cmd) + + rossleep(loop_rate) + end + println("Exiting open loop control.") +end + +if ! isinteractive() + main() +end From 61bbe8b5436c4891e417a438051965a738e8b060 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 20 Oct 2016 14:19:17 -0700 Subject: [PATCH 072/183] Restructured logging strategy, fixed open loop simulator/recorder. --- workspace/src/barc/launch/barc_sim.launch | 6 +- .../src/barc/launch/open_loop_remote.launch | 4 +- workspace/src/barc/src/LMPC_node.jl | 1 + workspace/src/barc/src/barc_record.jl | 73 +-------------- workspace/src/barc/src/open_loop.jl | 88 +++++++++++++------ 5 files changed, 68 insertions(+), 104 deletions(-) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 7f095805..86e13a08 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -8,11 +8,11 @@ - + - - + + diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 4bdd8580..55788e34 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -13,9 +13,7 @@ - - - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index a08af715..0825d6f7 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -12,6 +12,7 @@ using JuMP using Ipopt using JLD +# log msg include("barc_lib/classes.jl") include("barc_lib/LMPC/MPC_models.jl") include("barc_lib/LMPC/coeffConstraintCost.jl") diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 8dcde44a..59b08b7c 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -24,78 +24,7 @@ using std_msgs.msg using marvelmind_nav.msg using JLD -# This type contains measurement data (time, values and a counter) -type Measurements{T} - i::Int64 # measurement counter - t::Array{Float64} # time data (when it was received by this recorder) - t_msg::Array{Float64} # time that the message was sent - z::Array{T} # measurement values -end - -# This function cleans the zeros from the type above once the simulation is finished -function clean_up(m::Measurements) - m.t = m.t[1:m.i-1] - m.t_msg = m.t_msg[1:m.i-1] - m.z = m.z[1:m.i-1,:] -end - -function Quat2Euler(q::Array{Float64}) - sol = zeros(Float64,3) - sol[1] = atan2(2*(q[1]*q[2]+q[3]*q[4]),1-2*(q[2]^2+q[3]^2)) - sol[2] = asin(2*(q[1]*q[3]-q[4]*q[2])) - sol[3] = atan2(2*(q[1]*q[4]+q[2]*q[3]),1-2*(q[3]^2+q[4]^2)) - return sol -end - -function ECU_callback(msg::ECU,cmd_log::Measurements) - cmd_log.t[cmd_log.i] = to_sec(get_rostime()) - cmd_log.t_msg[cmd_log.i] = to_sec(msg.header.stamp) - cmd_log.z[cmd_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) - cmd_log.i += 1 - nothing -end - -function ECU_PWM_callback(msg::ECU,cmd_pwm_log::Measurements) - cmd_pwm_log.t[cmd_pwm_log.i] = to_sec(get_rostime()) - cmd_pwm_log.t_msg[cmd_pwm_log.i] = to_sec(msg.header.stamp) - cmd_pwm_log.z[cmd_pwm_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) - cmd_pwm_log.i += 1 - nothing -end - -function IMU_callback(msg::Imu,imu_meas::Measurements) - imu_meas.t[imu_meas.i] = to_sec(get_rostime()) - imu_meas.t_msg[imu_meas.i] = to_sec(msg.header.stamp) - imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z; - Quat2Euler([msg.orientation.w;msg.orientation.x;msg.orientation.y;msg.orientation.z]); - msg.linear_acceleration.x;msg.linear_acceleration.y;msg.linear_acceleration.z]::Array{Float64} - imu_meas.i += 1 - nothing -end - -function GPS_callback(msg::hedge_pos,gps_meas::Measurements) - gps_meas.t[gps_meas.i] = to_sec(get_rostime()) - gps_meas.t_msg[gps_meas.i] = to_sec(msg.header.stamp) - gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] - gps_meas.i += 1 - nothing -end - -function pos_info_callback(msg::pos_info,pos_info_log::Measurements) - pos_info_log.t[pos_info_log.i] = to_sec(get_rostime()) - pos_info_log.t_msg[pos_info_log.i] = to_sec(msg.header.stamp) - pos_info_log.z[pos_info_log.i,:] = [msg.s;msg.ey;msg.epsi;msg.v;msg.s_start;msg.x;msg.y;msg.v_x;msg.v_y;msg.psi;msg.psiDot] - pos_info_log.i += 1 - nothing -end - -function vel_est_callback(msg::Vel_est,vel_est_log::Measurements) - vel_est_log.t[vel_est_log.i] = to_sec(get_rostime()) - vel_est_log.t_msg[vel_est_log.i] = to_sec(msg.header.stamp) - vel_est_log.z[vel_est_log.i] = convert(Float64,msg.vel_est) - vel_est_log.i += 1 - nothing -end +include("barc_lib/log_functions.jl") function main() diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 08f03e09..c9bbd03b 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -1,41 +1,65 @@ #!/usr/bin/env julia using RobotOS -@rosimport barc.msg: ECU +@rosimport barc.msg: ECU, pos_info, Vel_est +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +@rosimport marvelmind_nav.msg: hedge_pos rostypegen() using barc.msg +using data_service.msg +using geometry_msgs.msg +using sensor_msgs.msg +using std_msgs.msg +using marvelmind_nav.msg +using JLD +include("barc_lib/log_functions.jl") +function main() -# This type contains measurement data (time, values and a counter) -type Measurements{T} - i::Int64 # measurement counter - t::Array{Float64} # time data (when it was received by this recorder) - t_msg::Array{Float64} # time that the message was sent - z::Array{T} # measurement values -end - -# This function cleans the zeros from the type above once the simulation is finished -function clean_up(m::Measurements) - m.t = m.t[1:m.i-1] - m.t_msg = m.t_msg[1:m.i-1] - m.z = m.z[1:m.i-1,:] -end - + # Set up logging + buffersize = 60000 + gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) + cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,11)) -function main() # Initialize ROS node and topics init_node("mpc_traj") - loop_rate = Rate(10) - pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} - t0_ros = get_rostime() - t0 = to_sec(t0_ros) - cmd = ECU() # command type - cmd.motor = 0 - cmd.servo = 0 + s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} + s3 = Subscriber("ecu_pwm", ECU, ECU_PWM_callback, (cmd_pwm_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} + s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} + + run_id = get_param("run_id") + + loop_rate = Rate(10) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + cmd = ECU() # command type + cmd.motor = 0 + cmd.servo = 0 cmd.header.stamp = get_rostime() - println("Starting open loop control.") + println("Starting open loop control. Wating.") + rossleep(3.0) + + t0_ros = get_rostime() + t0 = to_sec(t0_ros) + t = 0.0 + + gps_meas.i = 1 + imu_meas.i = 1 + cmd_log.i = 1 + cmd_pwm_log.i = 1 + vel_est_log.i = 1 + pos_info_log.i = 1 + # Start node - while ! is_shutdown() + while t < 7.0 # run exactly 7 seconds t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 0 @@ -53,7 +77,19 @@ function main() rossleep(loop_rate) end + + # Clean up buffers + clean_up(gps_meas) + clean_up(imu_meas) + clean_up(cmd_log) + clean_up(cmd_pwm_log) + clean_up(pos_info_log) + clean_up(vel_est_log) + println("Exiting open loop control.") + log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4]).jld" + save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) + println("Exiting node... Saved recorded data to $log_path.") end if ! isinteractive() From 344252a293a7b7ddecfde629dbbef2b325621a57 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 20 Oct 2016 14:22:41 -0700 Subject: [PATCH 073/183] Updated submodule. --- workspace/src/barc/src/barc_lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index c0a69ccb..11e205e3 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit c0a69ccb330a3d1d63a17d47224d16194dc382c9 +Subproject commit 11e205e3dd8caa9a5a1c57c5115caa5b3b29e86c From 430948b9708cc3ebc26453d0fd9c1845c8b1aa40 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 20 Oct 2016 17:44:55 -0700 Subject: [PATCH 074/183] Deleted obsolete messages (fixes rosserial make_libraries bug). --- workspace/src/barc/CMakeLists.txt | 5 ----- workspace/src/barc/msg/Encoder.msg | 4 ---- workspace/src/barc/msg/Logging.msg | 17 ----------------- workspace/src/barc/msg/Ultrasound.msg | 4 ---- workspace/src/barc/msg/Z_DynBkMdl.msg | 6 ------ workspace/src/barc/msg/Z_KinBkMdl.msg | 4 ---- workspace/src/barc/msg/state_info.msg | 6 ------ 7 files changed, 46 deletions(-) delete mode 100644 workspace/src/barc/msg/Encoder.msg delete mode 100644 workspace/src/barc/msg/Logging.msg delete mode 100644 workspace/src/barc/msg/Ultrasound.msg delete mode 100644 workspace/src/barc/msg/Z_DynBkMdl.msg delete mode 100644 workspace/src/barc/msg/Z_KinBkMdl.msg delete mode 100644 workspace/src/barc/msg/state_info.msg diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index 48e541fa..b864a6d3 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -48,12 +48,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES pos_info.msg - Logging.msg - Ultrasound.msg - Encoder.msg ECU.msg - Z_KinBkMdl.msg - Z_DynBkMdl.msg Vel_est.msg ) diff --git a/workspace/src/barc/msg/Encoder.msg b/workspace/src/barc/msg/Encoder.msg deleted file mode 100644 index 11fe3098..00000000 --- a/workspace/src/barc/msg/Encoder.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 FL -float32 FR -float32 BL -float32 BR diff --git a/workspace/src/barc/msg/Logging.msg b/workspace/src/barc/msg/Logging.msg deleted file mode 100644 index 431691e2..00000000 --- a/workspace/src/barc/msg/Logging.msg +++ /dev/null @@ -1,17 +0,0 @@ -# This is a message to hold data for the ECU (electronic control unit) -# -# Units may vary depending on the topic -# The motor controls the speeds of the vehicle through an input torque. (For input force, divide by radius of tire) -# The servo controls the steering angle -# -# For modeling and state estimation, motors units are [N], and servo units are [rad] -# For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle -float32 obj_val -string status -float32 solve_time -float32[] s -float32[] ey -float32[] epsi -float32[] v -float32[] a -float32[] d_f diff --git a/workspace/src/barc/msg/Ultrasound.msg b/workspace/src/barc/msg/Ultrasound.msg deleted file mode 100644 index 2ffa3263..00000000 --- a/workspace/src/barc/msg/Ultrasound.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 front -float32 back -float32 right -float32 left diff --git a/workspace/src/barc/msg/Z_DynBkMdl.msg b/workspace/src/barc/msg/Z_DynBkMdl.msg deleted file mode 100644 index 9b1e9e24..00000000 --- a/workspace/src/barc/msg/Z_DynBkMdl.msg +++ /dev/null @@ -1,6 +0,0 @@ -float64 x -float64 y -float64 v_x -float64 v_y -float64 psi -float64 psi_dot \ No newline at end of file diff --git a/workspace/src/barc/msg/Z_KinBkMdl.msg b/workspace/src/barc/msg/Z_KinBkMdl.msg deleted file mode 100644 index d76a6d56..00000000 --- a/workspace/src/barc/msg/Z_KinBkMdl.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 x -float32 y -float32 psi -float32 v diff --git a/workspace/src/barc/msg/state_info.msg b/workspace/src/barc/msg/state_info.msg deleted file mode 100644 index 28bdeb8d..00000000 --- a/workspace/src/barc/msg/state_info.msg +++ /dev/null @@ -1,6 +0,0 @@ -float64 x -float64 y -float64 xDot -float64 yDot -float64 psi -float64 psiDot \ No newline at end of file From 5541ee4e43b30075d7bbb52aba3f166d42841acf Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 20 Oct 2016 18:27:14 -0700 Subject: [PATCH 075/183] Fixed arduino code. --- .../arduino_nano328_node.ino | 29 +------------------ .../src/barc/launch/sensor_test_barc.launch | 6 ++-- 2 files changed, 4 insertions(+), 31 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index 915b1933..cb66bc2c 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -23,10 +23,8 @@ WARNING: #include //#include #include -#include #include #include -#include "Maxbotix.h" #include #include #include @@ -190,39 +188,14 @@ volatile unsigned long t0; // Global message variables // Encoder, RC Inputs, Electronic Control Unit, Ultrasound barc::ECU ecu; -//barc::ECU rc_inputs; -//barc::Encoder encoder; -//barc::Ultrasound ultrasound; - -//std_msgs::Int32 encoder_dt_FL; //(ADDED BY TOMMI 7JULY2016) -//std_msgs::Int32 encoder_dt_FR; //(ADDED BY TOMMI 7JULY2016) -//std_msgs::Int32 encoder_dt_BL; //(ADDED BY TOMMI 7JULY2016) -//std_msgs::Int32 encoder_dt_BR; //(ADDED BY TOMMI 7JULY2016) -//std_msgs::Float32 vel_est; // estimation of current velocity to be published barc::Vel_est vel_est; ros::NodeHandle nh; -//ros::Publisher pub_encoder("encoder", &encoder); -//ros::Publisher pub_rc_inputs("rc_inputs", &rc_inputs); -//ros::Publisher pub_ultrasound("ultrasound", &ultrasound); ros::Subscriber sub_ecu("ecu_pwm", ecuCallback); -//ros::Publisher pub_encoder_dt_FL("fl", &encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) -//ros::Publisher pub_encoder_dt_FR("fr", &encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) -//ros::Publisher pub_encoder_dt_BL("bl", &encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) -//ros::Publisher pub_encoder_dt_BR("br", &encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) ros::Publisher pub_vel_est("vel_est", &vel_est); // vel est publisher - -// Set up ultrasound sensors -/* -Maxbotix us_fr(14, Maxbotix::PW, Maxbotix::LV); // front -Maxbotix us_bk(15, Maxbotix::PW, Maxbotix::LV); // back -Maxbotix us_rt(16, Maxbotix::PW, Maxbotix::LV); // right -Maxbotix us_lt(17, Maxbotix::PW, Maxbotix::LV); // left -*/ - /************************************************************************** ARDUINO INITIALIZATION **************************************************************************/ @@ -265,7 +238,7 @@ void loop() { car.readAndCopyInputs(); vel_est.vel_est = car.getVelocityEstimate(); - vel_est.stamp = ros::Time::now(); + vel_est.header.stamp = nh.now(); pub_vel_est.publish(&vel_est); // publish estimated velocity ////////////////////////////////////////////////!!!! diff --git a/workspace/src/barc/launch/sensor_test_barc.launch b/workspace/src/barc/launch/sensor_test_barc.launch index 23a93a2e..b55982e5 100644 --- a/workspace/src/barc/launch/sensor_test_barc.launch +++ b/workspace/src/barc/launch/sensor_test_barc.launch @@ -23,17 +23,17 @@ - + - + - + From 93d498210cd319f0f99fba89debb36fb0f3b411d Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Tue, 25 Oct 2016 09:38:14 -0700 Subject: [PATCH 076/183] Added variables to pos_info, adapted Arduino code to 3-wheel decoders. --- .gitmodules | 6 +- .../arduino_nano328_node.ino | 2 +- eval_sim/eval_data.jl | 66 +++++++++++++++++-- .../src/barc/launch/open_loop_remote.launch | 4 +- workspace/src/barc/msg/pos_info.msg | 5 ++ workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_record.jl | 4 +- workspace/src/barc/src/barc_simulator_dyn.jl | 10 +-- workspace/src/barc/src/open_loop.jl | 11 ++-- .../src/barc/src/state_estimation_DynBkMdl.py | 3 +- 10 files changed, 90 insertions(+), 23 deletions(-) diff --git a/.gitmodules b/.gitmodules index b30135c1..8761a04b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "workspace/src/barc/src/LMPC_lib"] - path = workspace/src/barc/src/LMPC_lib - url = https://github.com/maxb91/LMPC_lib +[submodule "workspace/src/barc/src/barc_lib"] + path = workspace/src/barc/src/barc_lib + url = https://github.com/maxb91/barc_lib diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index cb66bc2c..f3c9c2d8 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -441,7 +441,7 @@ unsigned long Car::getEncoder_dTime_BR() { //(ADDE float Car::getVelocityEstimate() { if(FL_DeltaTime > 0 && FR_DeltaTime > 0) { - return 2.0*3.141593*0.036/2.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime)*1000000.0/2.0; // calculate current speed in m/s + return 2.0*3.141593*0.036/2.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime + 1.0/BR_DeltaTime)*1000000.0/3.0; // calculate current speed in m/s //return 1.0; } else { diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 7d8ae9f1..4ac838d4 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -12,6 +12,11 @@ using HDF5, JLD, ProfileView # pos_info[9] = v_y # pos_info[10] = psi # pos_info[11] = psiDot +# pos_info[12] = x_raw +# pos_info[13] = y_raw +# pos_info[14] = psi_raw +# pos_info[15] = v_raw +# pos_info[16] = psi_drift include("../workspace/src/barc/src/barc_lib/classes.jl") @@ -90,6 +95,7 @@ end # **************************************************************** function eval_run(code::AbstractString) log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_record = "$(homedir())/open_loop/output-record-0ed5.jld" d_rec = load(log_path_record) imu_meas = d_rec["imu_meas"] @@ -131,22 +137,28 @@ function eval_run(code::AbstractString) figure() title("Comparison of psi") - plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*") - legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot"]) + plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*",pos_info.t-t0,pos_info.z[:,16],"-*") + legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift"]) grid() figure() title("Raw IMU orientation data") - plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6]) + plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6]) grid("on") legend(["w_x","w_y","w_z","roll","pitch","yaw"]) figure() title("v measurements and estimate") - plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z,"-x") + plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z,"-x",vel_est.t_msg-t0,vel_est.z,"-+") legend(["est_xDot","est_yDot","v_raw"]) grid() + figure() + title("Acceleration data") + plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") + legend(["a_x","a_y","a_z"]) + grid() + figure() title("s, eY and inputs") ax1=subplot(211) @@ -160,6 +172,52 @@ function eval_run(code::AbstractString) nothing end +function eval_open_loop(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = pos_info.t[1] + + figure() + title("Comparison speed and input") + ax3=subplot(211) + plot(vel_est.t-t0,vel_est.z,vel_est.t_msg-t0,vel_est.z) + grid("on") + subplot(212,sharex=ax3) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,1],cmd_pwm_log.t_msg-t0,cmd_pwm_log.z[:,1]) + grid("on") + + gps_speed_raw = diff(gps_meas.z)./diff(gps_meas.t) + gps_speed = [0;sqrt(gps_speed_raw[:,1].^2+gps_speed_raw[:,2].^2)] + figure() + title("Comparison GPS and encoder speed") + plot(vel_est.t-t0,vel_est.z,"-*",gps_meas.t-t0,gps_speed,"-x",pos_info.t_msg-t0,pos_info.z[:,4],"-+") + grid("on") + legend(["encoder","gps","estimator"]) + + figure() + title("Acceleration data") + plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") + legend(["a_x","a_y","a_z"]) + grid() + + figure() + title("Acceleration data") + plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") + legend(["a_x","a_y","a_z"]) + grid() + +end + + + # THIS FUNCTION EVALUATES MPC-SPECIFIC DATA # ***************************************** function eval_LMPC(code::AbstractString) diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 55788e34..2b0bfb2b 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -11,8 +11,6 @@ - - @@ -27,7 +25,7 @@ - + diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 2063d852..bfbf4811 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -12,6 +12,11 @@ float64 v_x float64 v_y float64 psi float64 psiDot +float64 x_raw +float64 y_raw +float64 psi_raw +float64 v_raw +float64 psi_drift float64[] coeffX float64[] coeffY float64[] coeffTheta diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 11e205e3..b7f435bb 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 11e205e3dd8caa9a5a1c57c5115caa5b3b29e86c +Subproject commit b7f435bb76419be048231d94bddbf1caac587b62 diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 59b08b7c..f1cf88ed 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -34,7 +34,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,11)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,16)) # initiate node, set up publisher / subscriber topics init_node("barc_record") @@ -45,7 +45,7 @@ function main() s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} - run_id = get_param("run_id") + run_id = get_param("run_id") println("Recorder running.") spin() # wait for callbacks until shutdown diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 7b771c09..328179ac 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -82,10 +82,10 @@ function main() i = 2 - dist_traveled = 0.0 + dist_traveled = randn(3) # encoder positions of three wheels last_updated = 0.0 - r_tire = 0.036 # radius from tire center to perimeter along magnets [m] + r_tire = 0.036 # radius from tire center to perimeter along magnets [m] imu_drift = 0.0 # simulates yaw-sensor drift over time (slow sine) @@ -141,6 +141,8 @@ function main() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt + imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end @@ -148,8 +150,8 @@ function main() # Velocity measurements dist_traveled += norm(diff(z_current[i-1:i,1:2])) if i%5 == 0 # 20 Hz - if dist_traveled >= vel_dist_update # only update if a magnet has passed the sensor - dist_traveled = 0 + if sum(dist_traveled .>= vel_dist_update)>=1 # only update if at least one of the magnets has passed the sensor + dist_traveled[dist_traveled.>=vel_dist_update] = 0 vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) end vel_est.header.stamp = t_ros diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index c9bbd03b..6216eb66 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -25,7 +25,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,11)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,16)) # Initialize ROS node and topics init_node("mpc_traj") @@ -64,9 +64,12 @@ function main() if t <= 3 cmd.motor = 0 cmd.servo = 0 - elseif t <= 5 - cmd.motor = 1 - cmd.servo = 0 + elseif t <= 4 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 0.3 + elseif t <= 6 + cmd.motor = 1.5 + cmd.servo = -0.3 else cmd.motor = 0 cmd.servo = 0 diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 3d89fcac..7f4303c7 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -246,7 +246,8 @@ def state_estimation(): # and then publish position info state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, l.coeffX.tolist(), l.coeffY.tolist(), + l.psi, l.psiDot, x_meas_pred, y_meas_pred, yaw, vel_est, psi_drift, + l.coeffX.tolist(), l.coeffY.tolist(), l.coeffTheta.tolist(), l.coeffCurvature.tolist())) # apply EKF From b8ef2c9d307a1fc62813e91f6d04d253174679ce Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 26 Oct 2016 18:37:51 -0700 Subject: [PATCH 077/183] Added Kalman filter simulation scripts. Changed estimation to adaptive outlier-resistant EKF. --- eval_sim/eval_data.jl | 18 +- eval_sim/sim_kalman.jl | 187 ++++++++++++++++++ workspace/src/barc/launch/barc_sim.launch | 2 +- .../src/barc/launch/open_loop_remote.launch | 2 +- workspace/src/barc/src/open_loop.jl | 11 +- .../src/barc/src/state_estimation_DynBkMdl.py | 80 +++++--- workspace/src/barc/src/system_models.py | 2 +- 7 files changed, 256 insertions(+), 46 deletions(-) create mode 100644 eval_sim/sim_kalman.jl diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 4ac838d4..19ebea10 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -186,9 +186,10 @@ function eval_open_loop(code::AbstractString) t0 = pos_info.t[1] figure() - title("Comparison speed and input") ax3=subplot(211) - plot(vel_est.t-t0,vel_est.z,vel_est.t_msg-t0,vel_est.z) + title("Comparison speed and input") + plot(vel_est.t-t0,vel_est.z,"-*",vel_est.t_msg-t0,vel_est.z,"-x") + legend(["vel_est"]) grid("on") subplot(212,sharex=ax3) plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,1],cmd_pwm_log.t_msg-t0,cmd_pwm_log.z[:,1]) @@ -209,11 +210,16 @@ function eval_open_loop(code::AbstractString) grid() figure() - title("Acceleration data") - plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") - legend(["a_x","a_y","a_z"]) - grid() + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,12:13],"-x",pos_info.t-t0,pos_info.z[:,6:7]) + legend(["x_gps","y_gps","x_filtered","y_filtered","x_est","y_est"]) + grid("on") + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*") + legend(["x_gps","y_gps"]) + xlabel("t [s]") + ylabel("Position [m]") + grid("on") end diff --git a/eval_sim/sim_kalman.jl b/eval_sim/sim_kalman.jl new file mode 100644 index 00000000..93e4dd03 --- /dev/null +++ b/eval_sim/sim_kalman.jl @@ -0,0 +1,187 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.00001]) +R = diagm([0.1,0.1,0.5,0.5]) + +function main(code::AbstractString) + global Q, R + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/25 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,4) + u = zeros(sz,4) + + P = zeros(5,5) + x_est = zeros(length(t),5) + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] + y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 + y_vel_est = vel_est.z[vel_est.t.>t[i]][1] + y[i,:] = [y_gps y_yaw y_vel_est] + #u[i,:] = cmd_pwm_log.z[cmd_pwm_log.t.>t[i],:][1,:] + u[i,1] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1]-94.14)/2.7678 + u[i,2] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1]-91.365)/105.6 + if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1] == 90 + u[i,1] = 0 + end + if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1] == 90 + u[i,2] = 0 + end + + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + R[1,1] = 1+10*gps_dist[i]^2 + R[2,2] = 1+10*gps_dist[i]^2 + if y[i,4]==y[i-1,4] + R[4,4] = 0.1 + else + R[4,4] = 0.1 + end + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) + end + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + figure() + ax=subplot(5,1,1) + for i=1:4 + subplot(5,1,i,sharex=ax) + plot(t,y[:,i],t,x_est[:,i],"-*") + grid("on") + end + subplot(5,1,5,sharex=ax) + plot(cmd_pwm_log.t,cmd_pwm_log.z) + grid("on") + + + figure() + subplot(2,1,1) + title("Comparison raw GPS data and estimate") + plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + xlabel("t [s]") + ylabel("Position [m]") + legend(["x_m","x_est"]) + grid("on") + subplot(2,1,2) + plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + xlabel("t [s]") + ylabel("Position [m]") + legend(["y_m","y_est"]) + grid("on") + + figure() + plot(t,gps_dist) + title("GPS distances") + grid("on") + # figure() + # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # grid("on") + # title("x-y-view") + figure() + plot(t-t0,x_est,pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + grid("on") + legend(["x","y","psi","v","psi_drift"]) + nothing +end + +function h(x,args) + C = [eye(4) zeros(4,1)] + #C[4,4] = 0 + return C*x +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta) + z[5]) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 86e13a08..84e16b29 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -23,7 +23,7 @@ - + diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 2b0bfb2b..4f47c23e 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -29,7 +29,7 @@ - + diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 6216eb66..601ad44b 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -65,16 +65,15 @@ function main() cmd.motor = 0 cmd.servo = 0 elseif t <= 4 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 0.3 - elseif t <= 6 - cmd.motor = 1.5 - cmd.servo = -0.3 + cmd.motor = 2.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 0 + elseif t <= 7 + cmd.motor = 0 + cmd.servo = 0 else cmd.motor = 0 cmd.servo = 0 end - cmd.header.stamp = get_rostime() publish(pub, cmd) diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 7f4303c7..48230f89 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -47,12 +47,12 @@ x_meas = 0 y_meas = 0 -x_hist = [0]*25 -y_hist = [0]*25 -gps_times = [0]*25 +#x_hist = [0]*25 +#y_hist = [0]*25 +#gps_times = [0]*25 -poly_x = [0]*3 # polynomial coefficients for x and y position measurement (2nd order) -poly_y = [0]*3 +#poly_x = [0]*3 # polynomial coefficients for x and y position measurement (2nd order) +#poly_y = [0]*3 t0 = 0 running = False @@ -70,19 +70,24 @@ def ecu_callback(data): def gps_callback(data): # units: [rad] and [rad/s] global x_meas, y_meas, t0 - x_meas_pred = polyval(poly_x, data.header.stamp.to_sec()-t0) # predict new position - y_meas_pred = polyval(poly_y, data.header.stamp.to_sec()-t0) - - if abs(x_meas_pred-data.x_m) < 0.5 and abs(y_meas_pred-data.y_m) < 0.5 or not running: # check for outlier - x_meas = data.x_m # data is given in cm - y_meas = data.y_m - - x_hist.append(x_meas) - y_hist.append(y_meas) - gps_times.append(data.header.stamp.to_sec()-t0) - x_hist.pop(0) - y_hist.pop(0) - gps_times.pop(0) + #current_t = rospy.get_rostime().to_sec() + x_meas = data.x_m + y_meas = data.y_m + #x_meas_pred = polyval(poly_x, data.header.stamp.to_sec()-t0) # predict new position + #y_meas_pred = polyval(poly_y, data.header.stamp.to_sec()-t0) + #x_meas_pred = polyval(poly_x, current_t-t0) + #y_meas_pred = polyval(poly_y, current_t-t0) + # if abs(x_meas_pred-data.x_m) < 0.5 and abs(y_meas_pred-data.y_m) < 0.5 or not running: # check for outlier + # x_meas = data.x_m # data is given in cm + # y_meas = data.y_m + + # x_hist.append(x_meas) + # y_hist.append(y_meas) + # #gps_times.append(data.header.stamp.to_sec()-t0) + # gps_times.append(current_t-t0) + # x_hist.pop(0) + # y_hist.pop(0) + # gps_times.pop(0) # imu measurement update def imu_callback(data): @@ -90,6 +95,7 @@ def imu_callback(data): global roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z global yaw_prev, yaw0, yaw global imu_times, psiDot_hist + current_t = rospy.get_rostime().to_sec() # get orientation from quaternion data, and convert to roll, pitch, yaw # extract angular velocity and linear acceleration data @@ -106,7 +112,8 @@ def imu_callback(data): else: yaw = yaw - yaw0 - imu_times.append(data.header.stamp.to_sec()-t0) + #imu_times.append(data.header.stamp.to_sec()-t0) + imu_times.append(current_t-t0) imu_times.pop(0) # extract angular velocity and linear acceleration data @@ -126,7 +133,8 @@ def vel_est_callback(data): vel_est = data.vel_est vel_est_hist.append(vel_est) vel_est_hist.pop(0) - vel_times.append(data.header.stamp.to_sec()-t0) + #vel_times.append(data.header.stamp.to_sec()-t0) + vel_times.append(rospy.get_rostime().to_sec()-t0) vel_times.pop(0) # state estimation node @@ -166,11 +174,11 @@ def state_estimation(): if psi_drift_active: z_EKF = zeros(5) # x, y, psi, v, psi_drift P = eye(5) # initial dynamics coveriance matrix - Q = diag([5.0, 5.0, 5.0, 10.0, 1.0])*dt + Q = diag([2.5, 2.5, 2.5, 2.5, 0.00025])*dt else: z_EKF = zeros(4) P = eye(4) # initial dynamics coveriance matrix - Q = diag([5.0, 5.0, 5.0, 5.0])*dt + Q = diag([2.5, 2.5, 2.5, 2.5])*dt if est_mode == 1: # use gps, IMU, and encoder print "Using GPS, IMU and encoders." @@ -194,6 +202,9 @@ def state_estimation(): d_f = 0 + x_pred = 0 + y_pred = 0 + while not rospy.is_shutdown(): ros_t = rospy.get_rostime() t = ros_t.to_sec()-t0 # current time @@ -203,18 +214,25 @@ def state_estimation(): # ***** EXTRAPOLATE MEASUREMENTS TO CURRENT TIME ************************** # update x and y polynomial: - t_matrix = vstack((gps_times, gps_times, ones(size(gps_times)))) - t_matrix[0] = t_matrix[0]**2 - poly_x = linalg.lstsq(t_matrix.T, x_hist)[0] - poly_y = linalg.lstsq(t_matrix.T, y_hist)[0] + #t_matrix = vstack((gps_times, gps_times, ones(size(gps_times)))) + #t_matrix[0] = t_matrix[0]**2 + #poly_x = linalg.lstsq(t_matrix.T, x_hist)[0] + #poly_y = linalg.lstsq(t_matrix.T, y_hist)[0] # calculate current position from interpolated measurements - x_meas_pred = polyval(poly_x, t) - y_meas_pred = polyval(poly_y, t) + #x_meas_pred = polyval(poly_x, t) + #y_meas_pred = polyval(poly_y, t) + x_meas_pred = x_meas + y_meas_pred = y_meas + sq_gps_dist = (x_meas-x_pred)**2 + (y_meas-y_pred)**2 + # make R values dependent on current measurement + R[0,0] = 1+10*sq_gps_dist + R[1,1] = 1+10*sq_gps_dist + # update velocity estimation polynomial: - t_matrix_vel = vstack((vel_times, ones(size(vel_times)))) - poly_vel = linalg.lstsq(t_matrix_vel.T, vel_est_hist)[0] - vel_est_pred = polyval(poly_vel, t) + #t_matrix_vel = vstack((vel_times, ones(size(vel_times)))) + #poly_vel = linalg.lstsq(t_matrix_vel.T, vel_est_hist)[0] + #vel_est_pred = polyval(poly_vel, t) # update IMU polynomial: t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index a2d461a4..b516892b 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -75,7 +75,7 @@ def f_KinBkMdl_psi_drift(z,u,vhMdl, dt, est_mode): # compute next state x_next = x + dt*( v*cos(psi + bta) ) y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*v/L_b*sin(bta) + psi_next = psi + dt*(v/L_b*sin(bta) + psi_drift) v_next = v + dt*(a - 0.63*sign(v)*v**2) psi_drift_next = psi_drift From 7525b968777f3887fe19e007db9057cf92797074 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 28 Oct 2016 12:02:01 -0700 Subject: [PATCH 078/183] Added Kalman variations for testing. --- eval_sim/sim_kalman.jl | 89 ++++-- eval_sim/sim_kalman_imu.jl | 294 ++++++++++++++++++ eval_sim/sim_kalman_mix.jl | 290 +++++++++++++++++ .../src/barc/launch/open_loop_remote.launch | 8 +- .../src/barc/src/controller_low_level.py | 4 +- workspace/src/barc/src/open_loop.jl | 33 +- .../src/barc/src/state_estimation_DynBkMdl.py | 46 +-- workspace/src/barc/src/system_models.py | 5 +- 8 files changed, 700 insertions(+), 69 deletions(-) create mode 100644 eval_sim/sim_kalman_imu.jl create mode 100644 eval_sim/sim_kalman_mix.jl diff --git a/eval_sim/sim_kalman.jl b/eval_sim/sim_kalman.jl index 93e4dd03..186ef945 100644 --- a/eval_sim/sim_kalman.jl +++ b/eval_sim/sim_kalman.jl @@ -4,8 +4,8 @@ using PyPlot using JLD -Q = diagm([0.1,0.1,0.1,0.1,0.00001]) -R = diagm([0.1,0.1,0.5,0.5]) +Q = diagm([0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,10.0]) function main(code::AbstractString) global Q, R @@ -29,7 +29,7 @@ function main(code::AbstractString) t = t0:dt:t_end sz = length(t) y = zeros(sz,4) - u = zeros(sz,4) + u = zeros(sz,2) P = zeros(5,5) x_est = zeros(length(t),5) @@ -37,31 +37,37 @@ function main(code::AbstractString) yaw0 = imu_meas.z[1,6] gps_dist = zeros(length(t)) + yaw_prev = yaw0 + for i=2:length(t) # Collect measurements and inputs for this iteration y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 + #a_x = imu_meas.z[imu_meas.t.>t[i],7][1] y_vel_est = vel_est.z[vel_est.t.>t[i]][1] y[i,:] = [y_gps y_yaw y_vel_est] - #u[i,:] = cmd_pwm_log.z[cmd_pwm_log.t.>t[i],:][1,:] - u[i,1] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1]-94.14)/2.7678 - u[i,2] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1]-91.365)/105.6 - if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1] == 90 - u[i,1] = 0 - end - if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1] == 90 - u[i,2] = 0 - end + y[:,3] = unwrap!(y[:,3]) + u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + #u[i,1] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1]-94.14)/2.7678 + #u[i,2] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1]-91.365)/105.6 + # if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1] == 90 + # u[i,1] = 0 + # end + # if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1] == 90 + # u[i,2] = 0 + # end # Adapt R-value of GPS according to distance to last point: gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - R[1,1] = 1+10*gps_dist[i]^2 - R[2,2] = 1+10*gps_dist[i]^2 - if y[i,4]==y[i-1,4] - R[4,4] = 0.1 + if gps_dist[i] > 0.3 + R[1,1] = 1+100*gps_dist[i]^2 + R[2,2] = 1+100*gps_dist[i]^2 else - R[4,4] = 0.1 + R[1,1] = 1 + R[2,2] = 1 end + + args = (u[i,:],dt,l_A,l_B) # Calculate new estimate @@ -75,7 +81,7 @@ function main(code::AbstractString) # plot(t,u) # grid("on") # legend(["a_x","d_f"]) - figure() + figure(1) ax=subplot(5,1,1) for i=1:4 subplot(5,1,i,sharex=ax) @@ -87,7 +93,7 @@ function main(code::AbstractString) grid("on") - figure() + figure(2) subplot(2,1,1) title("Comparison raw GPS data and estimate") plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") @@ -102,7 +108,7 @@ function main(code::AbstractString) legend(["y_m","y_est"]) grid("on") - figure() + figure(3) plot(t,gps_dist) title("GPS distances") grid("on") @@ -110,16 +116,35 @@ function main(code::AbstractString) # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") # grid("on") # title("x-y-view") - figure() - plot(t-t0,x_est,pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + figure(4) + plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + title("Comparison simulation (-x) onboard-estimate (-*)") grid("on") legend(["x","y","psi","v","psi_drift"]) + + unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + figure(5) + plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + title("Comparison yaw") + grid("on") + + figure(6) + plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + grid("on") + title("Comparison of velocity measurement and estimate") + legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") nothing end function h(x,args) C = [eye(4) zeros(4,1)] - #C[4,4] = 0 + C[4,4] = 0 + #C[3,3] = 0 + C[3,5] = 1 return C*x end function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) @@ -146,9 +171,10 @@ function simModel(z,args) zNext = copy(z) zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta) + z[5]) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v - #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v zNext[5] = z[5] # psi_drift return zNext end @@ -185,3 +211,14 @@ function initPlot() # Initialize Plots for export rc("figure",figsize=[4.5,3]) #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end \ No newline at end of file diff --git a/eval_sim/sim_kalman_imu.jl b/eval_sim/sim_kalman_imu.jl new file mode 100644 index 00000000..121b7703 --- /dev/null +++ b/eval_sim/sim_kalman_imu.jl @@ -0,0 +1,294 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) + +R_gps_imu = diagm([0.1,0.1,1.0,1.0,0.1,0.1,0.01]) + +function main(code::AbstractString) + global Q, R, R_gps_imu + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/25 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,7) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(11,11) + x_est_gps_imu = zeros(length(t),11) + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + + Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] + y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 + y_yawdot = imu_meas.z[imu_meas.t.>t[i],3][1] + a_x = imu_meas.z[imu_meas.t.>t[i],7][1] + a_y = imu_meas.z[imu_meas.t.>t[i],8][1] + y_vel_est = vel_est.z[vel_est.t.>t[i]][1] + + y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] + y_gps_imu[i,:] = [y_gps a_x a_y y_yaw y_yawdot y_vel_est] + y[:,3] = unwrap!(y[:,3]) + y_gps_imu[:,5] = unwrap!(y_gps_imu[:,5]) + + u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + if gps_dist[i] > 0.3 + R[1,1] = 1+100*gps_dist[i]^2 + R[2,2] = 1+100*gps_dist[i]^2 + R_gps_imu[1,1] = 1+100*gps_dist[i]^2 + R_gps_imu[2,2] = 1+100*gps_dist[i]^2 + else + R[1,1] = 1 + R[2,2] = 1 + R_gps_imu[1,1] = 1 + R_gps_imu[2,2] = 1 + end + + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) + (x_est_gps_imu[i,:], P_gps_imu) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) + grid("on") + title("Comparison x,y estimate and measurement") + + figure(2) + plot(t-t0,x_est_gps_imu[:,9:11]) + grid("on") + title("Drifts") + legend(["a_x","a_y","psi"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z) + grid("on") + title("Velocity estimate and measurement") + + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + # figure(1) + # ax=subplot(5,1,1) + # for i=1:4 + # subplot(5,1,i,sharex=ax) + # plot(t,y[:,i],t,x_est[:,i],"-*") + # grid("on") + # end + # subplot(5,1,5,sharex=ax) + # plot(cmd_pwm_log.t,cmd_pwm_log.z) + # grid("on") + + + # figure(2) + # subplot(2,1,1) + # title("Comparison raw GPS data and estimate") + # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["x_m","x_est"]) + # grid("on") + # subplot(2,1,2) + # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["y_m","y_est"]) + # grid("on") + + # figure(3) + # plot(t,gps_dist) + # title("GPS distances") + # grid("on") + # # figure() + # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # # grid("on") + # # title("x-y-view") + # figure(4) + # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + # title("Comparison simulation (-x) onboard-estimate (-*)") + # grid("on") + # legend(["x","y","psi","v","psi_drift"]) + + # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + # figure(5) + # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + # title("Comparison yaw") + # grid("on") + + # figure(6) + # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + # grid("on") + # title("Comparison of velocity measurement and estimate") + # legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h(x,args) + C = [eye(6) zeros(6,1)] + C[4,4] = 1 + #C[3,3] = 0 + C[3,5] = 1 + C[5,5] = 0 + C[5,6] = 1 + C[6,6] = 0 + C[6,7] = 1 + return C*x +end +function h_gps_imu(x,args) + C = [1 0 0 0 0 0 0 0 0 0 0; # x + 0 1 0 0 0 0 0 0 0 0 0; # y + 0 0 0 0 1 0 0 0 1 0 0; # a_x + 0 0 0 0 0 1 0 0 0 1 0; # a_y + 0 0 0 0 0 0 1 0 0 0 1; # psi + 0 0 0 0 0 0 0 1 0 0 0; # psi_dot + 0 0 0 0 0 0 0 0 0 0 0] # v + y = C*x + y[7] = sqrt(x[3]^2+x[4]^2) # Idea: v_x_meas = cos(beta)*v, v_y_meas = sin(beta)*v + #return C*x + return y +end + +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + zNext[6] = z[6] # a_x + zNext[7] = z[7] # a_y + return zNext +end + +function simModel_gps_imu(z,args) + zNext = copy(z) + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x + zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y + #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*(z[5]+z[8]*z[4])-sin(z[7])*(z[6]-z[8]*z[3])) # x + #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*(z[5]+z[8]*z[4])+cos(z[7])*(z[6]-z[8]*z[3])) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + zNext[7] = z[7] + dt*(z[3]/l_B*sin(bta)) # psi + #zNext[7] = z[7] + dt*z[8] # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_a_x + zNext[10] = z[10] # drift_a_y + zNext[11] = z[11] # drift_psi + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end \ No newline at end of file diff --git a/eval_sim/sim_kalman_mix.jl b/eval_sim/sim_kalman_mix.jl new file mode 100644 index 00000000..f0251a5a --- /dev/null +++ b/eval_sim/sim_kalman_mix.jl @@ -0,0 +1,290 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) + + +function main(code::AbstractString) + global Q, R, R_gps_imu + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/25 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,7) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(11,11) + x_est_gps_imu = zeros(length(t),11) + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.01,0.01,0.01]) + Q_gps_imu = diagm([0.001,0.001,1/2*dt^2,1/2*dt^2,0.001,0.01,0.01,0.01,0.00001,0.001,0.001]) + R_gps_imu = diagm([0.1,0.1,0.1,0.1,0.1,5.0,5.0]) + + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] + y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 + y_yawdot = imu_meas.z[imu_meas.t.>t[i],3][1] + a_x = imu_meas.z[imu_meas.t.>t[i],7][1] + a_y = imu_meas.z[imu_meas.t.>t[i],8][1] + y_vel_est = vel_est.z[vel_est.t.>t[i]][1] + + y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] + y[:,3] = unwrap!(y[:,3]) + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + + u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + if gps_dist[i] > 0.3 + R[1,1] = 1+10*gps_dist[i]^2 + R[2,2] = 1+10*gps_dist[i]^2 + R_gps_imu[1,1] = 1+10*gps_dist[i]^2 + R_gps_imu[2,2] = 1+10*gps_dist[i]^2 + else + R[1,1] = 1 + R[2,2] = 1 + R_gps_imu[1,1] = 1 + R_gps_imu[2,2] = 1 + end + + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) + (x_est_gps_imu[i,:], P_gps_imu) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) + grid("on") + title("Comparison x,y estimate and measurement") + + figure(2) + plot(t-t0,x_est_gps_imu[:,9:11]) + grid("on") + title("Drifts") + legend(["a_x","a_y","psi"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z) + grid("on") + title("Velocity estimate and measurement") + + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + # figure(1) + # ax=subplot(5,1,1) + # for i=1:4 + # subplot(5,1,i,sharex=ax) + # plot(t,y[:,i],t,x_est[:,i],"-*") + # grid("on") + # end + # subplot(5,1,5,sharex=ax) + # plot(cmd_pwm_log.t,cmd_pwm_log.z) + # grid("on") + + + # figure(2) + # subplot(2,1,1) + # title("Comparison raw GPS data and estimate") + # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["x_m","x_est"]) + # grid("on") + # subplot(2,1,2) + # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["y_m","y_est"]) + # grid("on") + + # figure(3) + # plot(t,gps_dist) + # title("GPS distances") + # grid("on") + # # figure() + # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # # grid("on") + # # title("x-y-view") + # figure(4) + # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + # title("Comparison simulation (-x) onboard-estimate (-*)") + # grid("on") + # legend(["x","y","psi","v","psi_drift"]) + + # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + # figure(5) + # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + # title("Comparison yaw") + # grid("on") + + # figure(6) + # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + # grid("on") + # title("Comparison of velocity measurement and estimate") + # legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h(x,args) + C = [eye(6) zeros(6,1)] + C[4,4] = 1 + #C[3,3] = 0 + C[3,5] = 1 + C[5,5] = 0 + C[5,6] = 1 + C[6,6] = 0 + C[6,7] = 1 + return C*x +end +function h_gps_imu(x,args) + y = zeros(7) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[11] # psi + y[5] = x[8] # psiDot + y[6] = x[5]+x[9] # a_x + y[7] = x[6]+x[10] # a_y + return y +end + +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + zNext[6] = z[6] # a_x + zNext[7] = z[7] # a_y + return zNext +end + +function simModel_gps_imu(z,args) + zNext = copy(z) + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + zNext[1] = z[1] + dt*(sqrt(z[3]^2+z[4]^2)*cos(z[7] + bta)) # x + zNext[2] = z[2] + dt*(sqrt(z[3]^2+z[4]^2)*sin(z[7] + bta)) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + zNext[7] = z[7] + dt*(sqrt(z[3]^2+z[4]^2)/l_B*sin(bta)) # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_a_x + zNext[10] = z[10] # drift_a_y + zNext[11] = z[11] # drift_psi + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end \ No newline at end of file diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 4f47c23e..6b8f261b 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -18,10 +18,10 @@ - - + + - + @@ -29,7 +29,7 @@ - + diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 9d7345cd..cc669296 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -47,9 +47,11 @@ def pwm_converter_callback(self, msg): self.motor_pwm = 93.5 + 46.73*self.FxR self.update_arduino() def neutralize(self): - self.motor_pwm = 90 + self.motor_pwm = 60 # slow down first self.servo_pwm = 90 self.update_arduino() + self.motor_pwm = 90 + self.update_arduino() def update_arduino(self): self.ecu_cmd.header.stamp = get_rostime() self.ecu_cmd.motor = self.motor_pwm diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 601ad44b..cf2eb40d 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -59,16 +59,37 @@ function main() pos_info_log.i = 1 # Start node - while t < 7.0 # run exactly 7 seconds + while t < 44.0 # run exactly 7 seconds t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 0 cmd.servo = 0 - elseif t <= 4 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 2.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 0 - elseif t <= 7 - cmd.motor = 0 + elseif t <= 8 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.15 + elseif t <= 13 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.2 + elseif t <= 18 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.25 + elseif t <= 23 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.3 + elseif t <= 28 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.35 + elseif t <= 33 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.4 + elseif t <= 38 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.45 + elseif t <= 43 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = -0.5 + elseif t <= 44 + cmd.motor = -2.0 cmd.servo = 0 else cmd.motor = 0 diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 48230f89..4fee3c8a 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -40,8 +40,6 @@ # Velocity vel_est = 0 -vel_est_hist = [0]*2 -vel_times = [0]*2 # GPS x_meas = 0 @@ -131,11 +129,6 @@ def vel_est_callback(data): global vel_est, t0, vel_est_hist, vel_times if not data.vel_est == vel_est or not running: # if we're receiving a new measurement vel_est = data.vel_est - vel_est_hist.append(vel_est) - vel_est_hist.pop(0) - #vel_times.append(data.header.stamp.to_sec()-t0) - vel_times.append(rospy.get_rostime().to_sec()-t0) - vel_times.pop(0) # state estimation node def state_estimation(): @@ -211,16 +204,9 @@ def state_estimation(): # calculate new steering angle (low pass filter on steering input to make v_y and v_x smoother) d_f = d_f + (cmd_servo-d_f)*0.25 + #print d_f - # ***** EXTRAPOLATE MEASUREMENTS TO CURRENT TIME ************************** - # update x and y polynomial: - #t_matrix = vstack((gps_times, gps_times, ones(size(gps_times)))) - #t_matrix[0] = t_matrix[0]**2 - #poly_x = linalg.lstsq(t_matrix.T, x_hist)[0] - #poly_y = linalg.lstsq(t_matrix.T, y_hist)[0] - # calculate current position from interpolated measurements - #x_meas_pred = polyval(poly_x, t) - #y_meas_pred = polyval(poly_y, t) + # GPS measurement update x_meas_pred = x_meas y_meas_pred = y_meas sq_gps_dist = (x_meas-x_pred)**2 + (y_meas-y_pred)**2 @@ -228,17 +214,12 @@ def state_estimation(): R[0,0] = 1+10*sq_gps_dist R[1,1] = 1+10*sq_gps_dist - - # update velocity estimation polynomial: - #t_matrix_vel = vstack((vel_times, ones(size(vel_times)))) - #poly_vel = linalg.lstsq(t_matrix_vel.T, vel_est_hist)[0] - #vel_est_pred = polyval(poly_vel, t) - # update IMU polynomial: - t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) - t_matrix_imu[0] = t_matrix_imu[0]**2 - poly_psiDot = linalg.lstsq(t_matrix_imu.T, psiDot_hist)[0] - psiDot_meas_pred = polyval(poly_psiDot, t) + #t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) + #t_matrix_imu[0] = t_matrix_imu[0]**2 + #poly_psiDot = linalg.lstsq(t_matrix_imu.T, psiDot_hist)[0] + #psiDot_meas_pred = polyval(poly_psiDot, t) + psiDot_meas_pred = psiDot_hist[-1] if psi_drift_active: (x, y, psi, v, psi_drift) = z_EKF # note, r = EKF estimate yaw rate @@ -249,10 +230,10 @@ def state_estimation(): dt_pred = 0.0 bta = arctan(L_f/(L_f+L_r)*tan(d_f)) - x_pred = x + dt_pred*(v*cos(psi + bta)) - y_pred = y + dt_pred*(v*sin(psi + bta)) - psi_pred = psi + dt_pred*v/L_r*sin(bta) - v_pred = v + dt_pred*(FxR - 0.63*sign(v)*v**2) + x_pred = x# + dt_pred*(v*cos(psi + bta)) + y_pred = y# + dt_pred*(v*sin(psi + bta)) + psi_pred = psi# + dt_pred*v/L_r*sin(bta) + v_pred = v# + dt_pred*(FxR - 0.63*sign(v)*v**2) v_x_pred = cos(bta)*v_pred v_y_pred = sin(bta)*v_pred @@ -260,7 +241,10 @@ def state_estimation(): # Update track position l.set_pos(x_pred, y_pred, psi_pred, v_x_pred, v_x_pred, v_y_pred, psi_dot_pred) # v = v_x - l.find_s() + #l.find_s() + l.s = 0 + l.epsi = 0 + l.s_start = 0 # and then publish position info state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index b516892b..0f92c26e 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -72,10 +72,13 @@ def f_KinBkMdl_psi_drift(z,u,vhMdl, dt, est_mode): # compute slip angle bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + #print "psi_drift_model:" + #print psi + #print bta # compute next state x_next = x + dt*( v*cos(psi + bta) ) y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*(v/L_b*sin(bta) + psi_drift) + psi_next = psi + dt*(v/L_b*sin(bta))# + 0*psi_drift) v_next = v + dt*(a - 0.63*sign(v)*v**2) psi_drift_next = psi_drift From 17a2cccb6bed643114ef00fda9186af737afbb5c Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 2 Nov 2016 15:51:47 -0700 Subject: [PATCH 079/183] Added more evaluation scripts and Kalman filter variations. Adapted velocity message and arduino code so that all wheel speed estimates are published. --- .../arduino_nano328_node.ino | 23 +- eval_sim/eval_data.jl | 7 +- eval_sim/find_d_f_delay.jl | 43 ++ eval_sim/find_d_f_mapping.jl | 16 + eval_sim/sim_kalman_mix.jl | 126 ++++- eval_sim/sim_kalman_mix_nodrift.jl | 443 ++++++++++++++++++ workspace/src/barc/launch/barc_sim.launch | 8 +- .../src/barc/launch/open_loop_remote.launch | 2 +- .../src/barc/launch/run_lmpc_remote.launch | 2 +- workspace/src/barc/msg/Vel_est (copy).msg | 2 + workspace/src/barc/msg/Vel_est.msg | 6 +- workspace/src/barc/src/LMPC_node.jl | 19 +- .../src/barc/src/Localization_helpers.py | 17 +- workspace/src/barc/src/barc_simulator_dyn.jl | 6 +- .../src/barc/src/controller_low_level.py | 3 +- workspace/src/barc/src/open_loop.jl | 92 ++-- .../src/barc/src/state_estimation_DynBkMdl.py | 246 +++++----- .../src/state_estimation_DynBkMdl_mixed.py | 201 ++++++++ ...state_estimation_DynBkMdl_mixed_nodrift.py | 201 ++++++++ workspace/src/barc/src/system_models.py | 85 +++- 20 files changed, 1326 insertions(+), 222 deletions(-) create mode 100644 eval_sim/find_d_f_delay.jl create mode 100644 eval_sim/find_d_f_mapping.jl create mode 100644 eval_sim/sim_kalman_mix_nodrift.jl create mode 100644 workspace/src/barc/msg/Vel_est (copy).msg create mode 100755 workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py create mode 100755 workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index f3c9c2d8..ad0ac7e1 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -128,6 +128,11 @@ class Car { int BL_count = 0; int BR_count = 0; + float vel_FL = 0; + float vel_FR = 0; + float vel_BL = 0; + float vel_BR = 0; + // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) volatile unsigned long FL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) @@ -238,6 +243,10 @@ void loop() { car.readAndCopyInputs(); vel_est.vel_est = car.getVelocityEstimate(); + vel_est.vel_fl = car.vel_FL; + vel_est.vel_fr = car.vel_FR; + vel_est.vel_bl = car.vel_BL; + vel_est.vel_br = car.vel_BR; vel_est.header.stamp = nh.now(); pub_vel_est.publish(&vel_est); // publish estimated velocity ////////////////////////////////////////////////!!!! @@ -440,7 +449,19 @@ unsigned long Car::getEncoder_dTime_BR() { //(ADDE } //(ADDED BY TOMMI 7JULY2016) float Car::getVelocityEstimate() { - if(FL_DeltaTime > 0 && FR_DeltaTime > 0) { + if(FL_DeltaTime > 0){ + vel_FL = 2.0*3.141593*0.036/2.0*1.0/FL_DeltaTime; + } + if(FR_DeltaTime > 0){ + vel_FR = 2.0*3.141593*0.036/2.0*1.0/FR_DeltaTime; + } + if(BL_DeltaTime > 0){ + vel_BL = 2.0*3.141593*0.036/2.0*1.0/BL_DeltaTime; + } + if(BR_DeltaTime > 0){ + vel_BR = 2.0*3.141593*0.036/2.0*1.0/BR_DeltaTime; + } + if(FL_DeltaTime > 0 && FR_DeltaTime > 0 && BR_DeltaTime) { return 2.0*3.141593*0.036/2.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime + 1.0/BR_DeltaTime)*1000000.0/3.0; // calculate current speed in m/s //return 1.0; } diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 19ebea10..ebbe4e18 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -95,7 +95,7 @@ end # **************************************************************** function eval_run(code::AbstractString) log_path_record = "$(homedir())/simulations/output-record-$(code).jld" - log_path_record = "$(homedir())/open_loop/output-record-0ed5.jld" + #log_path_record = "$(homedir())/open_loop/output-record-0ed5.jld" d_rec = load(log_path_record) imu_meas = d_rec["imu_meas"] @@ -116,6 +116,11 @@ function eval_run(code::AbstractString) axis("equal") legend(["GPS meas","estimate"]) + figure() + plot(imu_meas.t-t0,imu_meas.z[:,7:9]) + grid("on") + title("Measured accelerations") + figure() title("x/y measurements and estimate") plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x") diff --git a/eval_sim/find_d_f_delay.jl b/eval_sim/find_d_f_delay.jl new file mode 100644 index 00000000..4af5a99a --- /dev/null +++ b/eval_sim/find_d_f_delay.jl @@ -0,0 +1,43 @@ + +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + delta = atan2(psiDot*0.25,v_x) + plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + xlabel("t [s]") + legend(["delta_true","delta_input"]) + + ax1=subplot(211) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,1])) + grid("on") + subplot(212,sharex=ax1) + plot(vel_est.t-t0,vel_est.z) + grid("on") +end \ No newline at end of file diff --git a/eval_sim/find_d_f_mapping.jl b/eval_sim/find_d_f_mapping.jl new file mode 100644 index 00000000..85de5664 --- /dev/null +++ b/eval_sim/find_d_f_mapping.jl @@ -0,0 +1,16 @@ + +using PyPlot +using JLD + +function main() + log_path_record_1 = "$(homedir())/open_loop/output-record-8bbb.jld" + log_path_record_2 = "$(homedir())/open_loop/output-record-3593.jld" + d_rec_1 = load(log_path_record) + d_rec_2 = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] diff --git a/eval_sim/sim_kalman_mix.jl b/eval_sim/sim_kalman_mix.jl index f0251a5a..7149af39 100644 --- a/eval_sim/sim_kalman_mix.jl +++ b/eval_sim/sim_kalman_mix.jl @@ -7,6 +7,23 @@ using JLD Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) +# pos_info[1] = s +# pos_info[2] = eY +# pos_info[3] = ePsi +# pos_info[4] = v +# pos_info[5] = s_start +# pos_info[6] = x +# pos_info[7] = y +# pos_info[8] = v_x +# pos_info[9] = v_y +# pos_info[10] = psi +# pos_info[11] = psiDot +# pos_info[12] = x_raw +# pos_info[13] = y_raw +# pos_info[14] = psi_raw +# pos_info[15] = v_raw +# pos_info[16] = psi_drift + function main(code::AbstractString) global Q, R, R_gps_imu @@ -38,6 +55,8 @@ function main(code::AbstractString) x_est = zeros(length(t),7) P_gps_imu = zeros(11,11) x_est_gps_imu = zeros(length(t),11) + #x_est_gps_imu[1,11] = 0.5 + #x_est_gps_imu[1,10] = -0.6 yaw0 = imu_meas.z[1,6] gps_dist = zeros(length(t)) @@ -45,37 +64,66 @@ function main(code::AbstractString) yaw_prev = yaw0 #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.01,0.01,0.01]) - Q_gps_imu = diagm([0.001,0.001,1/2*dt^2,1/2*dt^2,0.001,0.01,0.01,0.01,0.00001,0.001,0.001]) - R_gps_imu = diagm([0.1,0.1,0.1,0.1,0.1,5.0,5.0]) - + Q_gps_imu = diagm([0.01,0.01,1/2*dt^2,1/2*dt^2,0.001,0.001,0.01,0.01,0.00001,0.00001,0.00001]) + R_gps_imu = diagm([0.1,0.1,0.1,0.1,0.1,1.0,1.0]) + + att_raw = zeros(sz,3) + att_normalized = zeros(sz,3) + + acc_raw = zeros(sz,3) + acc_normalized = zeros(sz,3) + acc_inert = zeros(sz,3) + + omega_normalized = zeros(sz,3) + + # Calibrate IMU: + t_start = cmd_log.t[1] + imu_att_cal = mean(imu_meas.z[imu_meas.t.t[i],:][1,:] y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 y_yawdot = imu_meas.z[imu_meas.t.>t[i],3][1] - a_x = imu_meas.z[imu_meas.t.>t[i],7][1] - a_y = imu_meas.z[imu_meas.t.>t[i],8][1] + a_x = imu_meas.z[imu_meas.t.>t[i],7][1] + a_y = imu_meas.z[imu_meas.t.>t[i],8][1] y_vel_est = vel_est.z[vel_est.t.>t[i]][1] + acc_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],7:9][1,:] + att_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],4:6][1,:] + BI_A = eulerTrans([att_raw[i,1:2] 0]) + acc_normalized[i,:] = BI_A'*acc_raw[i,:]' + omega_normalized[i,:] = BI_A'*imu_meas.z[imu_meas.t.>t[i],1:3][1,:]' + #acc_inert[i,:] = eulerTrans([imu_meas.z[imu_meas.t.>t[i],4:5][1,:] y_yaw])'*acc_raw[i,:]' y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] + #y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot acc_normalized[i,1:2]] y[:,3] = unwrap!(y[:,3]) y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + #y_gps_imu[i,6:7] = [0 0] u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + #att_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],4:6][1,:] + #att_normalized[i,:] = Z_IMU_A' * att_raw[i,:]' + #att_normalized[i,:] = angRates2EulerRates(att_raw[i,:],[-imu_meas.z[imu_meas.t.>t[i],4:5][1,1:2] imu_meas.z[imu_meas.t.>t[i],6][1]]) + #att_normalized[i,:] = rotMatrix('y',imu_meas.z[imu_meas.t.>t[i],5][1])*rotMatrix('x',imu_meas.z[imu_meas.t.>t[i],4][1])*att_raw[i,:]' # Adapt R-value of GPS according to distance to last point: gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - if gps_dist[i] > 0.3 + if gps_dist[i] > 0.2 R[1,1] = 1+10*gps_dist[i]^2 R[2,2] = 1+10*gps_dist[i]^2 - R_gps_imu[1,1] = 1+10*gps_dist[i]^2 - R_gps_imu[2,2] = 1+10*gps_dist[i]^2 + R_gps_imu[1,1] = 0.1+100*gps_dist[i]^2 + R_gps_imu[2,2] = 0.1+100*gps_dist[i]^2 else R[1,1] = 1 R[2,2] = 1 - R_gps_imu[1,1] = 1 - R_gps_imu[2,2] = 1 + R_gps_imu[1,1] = 0.1 + R_gps_imu[2,2] = 0.1 end @@ -87,7 +135,7 @@ function main(code::AbstractString) end figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,pos_info.t-t0,pos_info.z[:,6:7],"-x") grid("on") title("Comparison x,y estimate and measurement") @@ -99,10 +147,24 @@ function main(code::AbstractString) figure(3) v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z,pos_info.t-t0,pos_info.z[:,8:9],"-x") grid("on") + legend(["v","v_x_est","v_y_est","v_meas"]) title("Velocity estimate and measurement") + figure(4) + plot(t-t0,x_est_gps_imu[:,7],"-*",t-t0,y_gps_imu[:,4]) + grid("on") + title("Comparison yaw") + + figure(5) + plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,8],"-*") + grid("on") + + figure(6) + #plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) + plot(t-t0,acc_normalized[:,1:2],t-t0,x_est_gps_imu[:,5:6]) + grid("on") # ax1=subplot(211) # plot(t,y,"-x",t,x_est,"-*") # grid("on") @@ -228,9 +290,10 @@ function simModel(z,args) end function simModel_gps_imu(z,args) - zNext = copy(z) (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) + #bta = atan(l_A/(l_A+l_B)*tan(u[2])) + bta = atan2(z[4],z[3]) + zNext = copy(z) zNext[1] = z[1] + dt*(sqrt(z[3]^2+z[4]^2)*cos(z[7] + bta)) # x zNext[2] = z[2] + dt*(sqrt(z[3]^2+z[4]^2)*sin(z[7] + bta)) # y zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x @@ -287,4 +350,37 @@ function unwrap!(p) end end return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end + +function angRates2EulerRates(w::Array{Float64},deg::Array{Float64}) + (p,q,r) = w + (phi,theta,psi) = deg + phidot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r + thetadot = cos(phi)*q - sin(phi)*r + psidot = sin(phi)*sec(theta)*q + cos(phi)*sec(theta)*r + return [phidot,thetadot,psidot] +end + +function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body + return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) end \ No newline at end of file diff --git a/eval_sim/sim_kalman_mix_nodrift.jl b/eval_sim/sim_kalman_mix_nodrift.jl new file mode 100644 index 00000000..cd153903 --- /dev/null +++ b/eval_sim/sim_kalman_mix_nodrift.jl @@ -0,0 +1,443 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) + +# pos_info[1] = s +# pos_info[2] = eY +# pos_info[3] = ePsi +# pos_info[4] = v +# pos_info[5] = s_start +# pos_info[6] = x +# pos_info[7] = y +# pos_info[8] = v_x +# pos_info[9] = v_y +# pos_info[10] = psi +# pos_info[11] = psiDot +# pos_info[12] = x_raw +# pos_info[13] = y_raw +# pos_info[14] = psi_raw +# pos_info[15] = v_raw +# pos_info[16] = psi_drift + + +function main(code::AbstractString) + global Q, R, R_gps_imu + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/50 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,7) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(9,9) + x_est_gps_imu = zeros(length(t),9) + #x_est_gps_imu[1,11] = 0.5 + #x_est_gps_imu[1,10] = -0.6 + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.01,0.01,0.01]) + #Q_gps_imu = diagm([0.001,0.001,0.001,0.001,0.001,0.001,0.01,0.1,0.00001]) + #R_gps_imu = diagm([0.1,0.1,0.1,0.001,0.01,0.01,0.01]) + + Q_gps_imu = diagm([0.01,0.01,0.1,0.1,0.1,0.1,0.01,0.1,0.0000001]) + R_gps_imu = diagm([0.1,0.1,10.0,0.01,1.0,10.0,10.0]) + + att_raw = zeros(sz,3) + att_normalized = zeros(sz,3) + + acc_raw = zeros(sz,3) + acc_normalized = zeros(sz,3) + acc_inert = zeros(sz,3) + + omega_normalized = zeros(sz,3) + + gps_gate = zeros(length(t)) + # Calibrate IMU: + t_start = cmd_log.t[1] + imu_att_cal = mean(imu_meas.z[imu_meas.t.gps_meas.t,:][end,:] + y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + y_vel_est = vel_est.z[t[i].>vel_est.t][end] + + acc_raw[i,:] = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + att_raw[i,:] = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] + #BI_A = eulerTrans([att_raw[i,1:2] 0]) + BI_A = eye(3) + acc_normalized[i,:] = BI_A'*acc_raw[i,:]' + omega_normalized[i,:] = BI_A'*imu_meas.z[t[i].>imu_meas.t,1:3][end,:]' + #acc_inert[i,:] = eulerTrans([imu_meas.z[imu_meas.t.>t[i],4:5][1,:] y_yaw])'*acc_raw[i,:]' + y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] + #y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw omega_normalized[i,3] acc_normalized[i,1:2]] + y[:,3] = unwrap!(y[:,3]) + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + #y_gps_imu[i,6:7] = [0 0] + + u[i,:] = cmd_log.z[t[i].>cmd_log.t,:][end,:] + + #att_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],4:6][1,:] + #att_normalized[i,:] = Z_IMU_A' * att_raw[i,:]' + #att_normalized[i,:] = angRates2EulerRates(att_raw[i,:],[-imu_meas.z[imu_meas.t.>t[i],4:5][1,1:2] imu_meas.z[imu_meas.t.>t[i],6][1]]) + #att_normalized[i,:] = rotMatrix('y',imu_meas.z[imu_meas.t.>t[i],5][1])*rotMatrix('x',imu_meas.z[imu_meas.t.>t[i],4][1])*att_raw[i,:]' + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + #gps_gate[i] = + # if gps_dist[i] > 1.0 + # R[1,1] = 1+10*gps_dist[i]^2 + # R[2,2] = 1+10*gps_dist[i]^2 + # R_gps_imu[1,1] = 1.0+100*gps_dist[i]^2 + # R_gps_imu[2,2] = 1.0+100*gps_dist[i]^2 + # else + # R[1,1] = 1 + # R[2,2] = 1 + # R_gps_imu[1,1] = 0.1 + # R_gps_imu[2,2] = 0.1 + # end + if gps_prev == y_gps + R_gps_imu[1:2,1:2] = 10*eye(2) + else + R_gps_imu[1:2,1:2] = 0.1*eye(2) + end + gps_prev = y_gps + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) + (x_est_gps_imu[i,:], P_gps_imu, gps_gate[i]) = ekf_gate(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,pos_info.t-t0,pos_info.z[:,6:7],"-x") + grid("on") + title("Comparison x,y estimate and measurement") + legend(["x_est","y_est","x_meas","y_meas","x_onboard","y_onboard"]) + + figure(2) + plot(t-t0,x_est_gps_imu[:,9]) + grid("on") + title("Drifts") + legend(["psi"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z,pos_info.t-t0,pos_info.z[:,8:9],"-x") + grid("on") + legend(["v","v_x_est","v_y_est","v_meas","v_x_onboard","v_y_onboard"]) + title("Velocity estimate and measurement") + + figure(4) + plot(t-t0,x_est_gps_imu[:,7],"-*",t-t0,y_gps_imu[:,4]) + grid("on") + title("Comparison yaw") + legend(["psi_est","psi_meas"]) + + figure(5) + plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,8]) + title("w_z") + legend(["w_z_meas","w_z_est"]) + grid("on") + + figure(6) + #plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) + plot(t-t0,acc_normalized[:,1:2],t-t0,x_est_gps_imu[:,5:6]) + title("Accelerations") + legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + grid("on") + + figure(7) + plot(t-t0,gps_gate,t-t0,gps_dist,gps_meas.t-t0,gps_meas.z) + grid("on") + legend(["gate","dist"]) + + # CORRELATIONS: + figure(8) + plot(94.14+2.7678*u[:,1],y[:,4],"*") + grid("on") + title("Comparison motor input and velocity") + xlabel("PWM signal") + ylabel("v [m/s]") + + delta = atan(x_est_gps_imu[:,8]*0.25./x_est_gps_imu[:,3]) + #delta2 = atan(0.25/sqrt(y_gps_imu[:,3].^2./y_gps_imu[:,5].^2-0.125^2)) + figure(9) + plot(u[:,2],delta,"*") + grid("on") + + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + # figure(1) + # ax=subplot(5,1,1) + # for i=1:4 + # subplot(5,1,i,sharex=ax) + # plot(t,y[:,i],t,x_est[:,i],"-*") + # grid("on") + # end + # subplot(5,1,5,sharex=ax) + # plot(cmd_pwm_log.t,cmd_pwm_log.z) + # grid("on") + + + # figure(2) + # subplot(2,1,1) + # title("Comparison raw GPS data and estimate") + # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["x_m","x_est"]) + # grid("on") + # subplot(2,1,2) + # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["y_m","y_est"]) + # grid("on") + + # figure(3) + # plot(t,gps_dist) + # title("GPS distances") + # grid("on") + # # figure() + # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # # grid("on") + # # title("x-y-view") + # figure(4) + # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + # title("Comparison simulation (-x) onboard-estimate (-*)") + # grid("on") + # legend(["x","y","psi","v","psi_drift"]) + + # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + # figure(5) + # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + # title("Comparison yaw") + # grid("on") + + # figure(6) + # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + # grid("on") + # title("Comparison of velocity measurement and estimate") + # legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h(x,args) + C = [eye(6) zeros(6,1)] + C[4,4] = 1 + #C[3,3] = 0 + C[3,5] = 1 + C[5,5] = 0 + C[5,6] = 1 + C[6,6] = 0 + C[6,7] = 1 + return C*x +end +function h_gps_imu(x,args) + y = zeros(7) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[9] # psi + y[5] = x[8] # psiDot + y[6] = x[5] # a_x + y[7] = x[6] # a_y + return y +end + +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function ekf_gate(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + S = H*P12 + gps_err = ((y_kp1[1:2]-my_kp1[1:2])'*inv(S[1:2,1:2])*(y_kp1[1:2]-my_kp1[1:2]))[1] + # if gps_err > 100.0 + # R[1:2,1:2] = eye(2)*100 + # else + # R[1:2,1:2] = eye(2)*0.1 + # end + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1, gps_err +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + zNext[6] = z[6] # a_x + zNext[7] = z[7] # a_y + return zNext +end + +function simModel_gps_imu(z,args) + (u,dt,l_A,l_B) = args + #bta = atan(l_A/(l_A+l_B)*tan(u[2])) + bta = atan2(z[4],z[3]) + zNext = copy(z) + zNext[1] = z[1] + dt*(sqrt(z[3]^2+z[4]^2)*cos(z[7] + bta)) # x + zNext[2] = z[2] + dt*(sqrt(z[3]^2+z[4]^2)*sin(z[7] + bta)) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + zNext[7] = z[7] + dt*(sqrt(z[3]^2+z[4]^2)/l_B*sin(bta)) # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_psi + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end + +function angRates2EulerRates(w::Array{Float64},deg::Array{Float64}) + (p,q,r) = w + (phi,theta,psi) = deg + phidot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r + thetadot = cos(phi)*q - sin(phi)*r + psidot = sin(phi)*sec(theta)*q + cos(phi)*sec(theta)*r + return [phidot,thetadot,psidot] +end + +function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body + return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) +end \ No newline at end of file diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 84e16b29..f6921a24 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -8,17 +8,17 @@ - + - - + + - + diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 6b8f261b..953d2514 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -13,7 +13,7 @@ - + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index cdbee9e5..b44ae570 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -17,7 +17,7 @@ - + diff --git a/workspace/src/barc/msg/Vel_est (copy).msg b/workspace/src/barc/msg/Vel_est (copy).msg new file mode 100644 index 00000000..1ade9b44 --- /dev/null +++ b/workspace/src/barc/msg/Vel_est (copy).msg @@ -0,0 +1,2 @@ +Header header +float32 vel_est \ No newline at end of file diff --git a/workspace/src/barc/msg/Vel_est.msg b/workspace/src/barc/msg/Vel_est.msg index 1ade9b44..69111051 100644 --- a/workspace/src/barc/msg/Vel_est.msg +++ b/workspace/src/barc/msg/Vel_est.msg @@ -1,2 +1,6 @@ Header header -float32 vel_est \ No newline at end of file +float32 vel_est +float32 vel_fl +float32 vel_fr +float32 vel_bl +float32 vel_br \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 0825d6f7..dfb56196 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -1,7 +1,7 @@ #!/usr/bin/env julia using RobotOS -@rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Logging +@rosimport barc.msg: ECU, pos_info @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 rostypegen() @@ -87,11 +87,11 @@ function main() # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) - pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} - run_id = get_param("run_id") + run_id = get_param("run_id") println("Finished initialization.") # Lap parameters switchLap = false # initialize lap lap trigger @@ -108,7 +108,11 @@ function main() lapStatus.currentIt = 1 posInfo.s_target = 12.0#24.0 k = 0 # overall counter for logging - mpcSol.z = zeros(10,4) + + mpcSol.z = zeros(11,4) + mpcSol.u = zeros(10,2) + mpcSol.a_x = 0 + mpcSol.d_f = 0 # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) @@ -128,11 +132,12 @@ function main() # ============================= PUBLISH COMMANDS ============================= # this is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received # the state is predicted by 0.1s + println("Publishing command...") cmd.header.stamp = get_rostime() cmd.motor = convert(Float32,mpcSol.a_x) cmd.servo = convert(Float32,mpcSol.d_f) publish(pub, cmd) - + println("Finished publishing.") # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information (actually predicted by Kalman filter!) @@ -190,7 +195,7 @@ function main() else last_u = u_final end - + println("Starting solving.") # Solve the MPC problem tic() if lapStatus.currentLap <= 2 diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index c58681a8..458d0196 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -3,7 +3,10 @@ #from numpy import sin, cos, tan, array, dot, linspace, pi, ones, zeros, amin, argmin, arange #from numpy import hstack, vstack, sign, size, diff, cumsum, mod, floor, interp, linalg #from numpy import polyval, polyder, arctan2 -from numpy import * +from numpy import hstack, arange, array, zeros, vstack, transpose, pi, linspace, sin, cos, inf +from numpy import size, tan, diff, cumsum, mod, argmin, interp, floor, arctan2, polyval, polyder +from numpy import amin, dot, ones, sqrt, linalg, sign, sum +#from numpy import * import scipy.optimize def create_circle(rad, n, c): @@ -29,8 +32,8 @@ def create_bezier(p0,p1,p2,dt): p = (1-t)**2*p0 + 2*(1-t)*t*p1+t**2*p2 return p -class Localization: - """ This is the main class which includes all variables and functions to calculate the current relative position.""" +class Localization(object): + """This is the main class which includes all variables and functions to calculate the current relative position.""" n = 0 # number of nodes c = 0 # center of circle (in case of circular trajectory) pos = 0 # current position @@ -232,10 +235,10 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e #print(self.nodes) - def set_pos(self,x,y,psi,v,v_x,v_y,psiDot): + def set_pos(self,x,y,psi,v_x,v_y,psiDot): self.pos = array([x,y]) self.psi = psi - self.v = v + self.v = sqrt(v_x**2+v_y**2) self.x = x self.y = y self.v_x = v_x @@ -245,7 +248,7 @@ def set_pos(self,x,y,psi,v,v_x,v_y,psiDot): def find_s(self): dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of current position to all nodes idx_min = argmin(dist) # index of minimum distance - #print("closest point: %f"%idx_min) + # print "closest point: %f"%idx_min n = self.n # number of nodes nPoints = self.nPoints # number of points for polynomial approximation (around current position) @@ -331,7 +334,7 @@ def find_s(self): # Calculate s discretization = 0.0001 # discretization to calculate s - j = arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) + j = arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) #print "idx_min = %f"%idx_min #print "s_idx_start = %f"%s_idx_start #print "idx_start = %f"%idx_start diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 328179ac..886245a2 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -141,8 +141,8 @@ function main() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros - imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] + randn()*0.5 + imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] + randn()*0.5 publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end @@ -150,7 +150,7 @@ function main() # Velocity measurements dist_traveled += norm(diff(z_current[i-1:i,1:2])) if i%5 == 0 # 20 Hz - if sum(dist_traveled .>= vel_dist_update)>=1 # only update if at least one of the magnets has passed the sensor + if sum(dist_traveled .>= vel_dist_update)>=1 && z_current[i,3] > 0.1 # only update if at least one of the magnets has passed the sensor dist_traveled[dist_traveled.>=vel_dist_update] = 0 vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) end diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index cc669296..9bfa80ef 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -21,7 +21,6 @@ from rospy import init_node, Subscriber, Publisher, get_param, get_rostime from rospy import Rate, is_shutdown, ROSInterruptException, spin, on_shutdown from barc.msg import ECU -from numpy import pi import rospy class low_level_control(object): @@ -50,6 +49,7 @@ def neutralize(self): self.motor_pwm = 60 # slow down first self.servo_pwm = 90 self.update_arduino() + rospy.sleep(1) # slow down for 1 sec self.motor_pwm = 90 self.update_arduino() def update_arduino(self): @@ -59,7 +59,6 @@ def update_arduino(self): self.ecu_pub.publish(self.ecu_cmd) def arduino_interface(): - # launch node, subscribe to motorPWM and servoPWM, publish ecu init_node('arduino_interface') llc = low_level_control() diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index cf2eb40d..d7f42aa7 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -44,7 +44,7 @@ function main() cmd.motor = 0 cmd.servo = 0 cmd.header.stamp = get_rostime() - println("Starting open loop control. Wating.") + println("Starting open loop control. Waiting.") rossleep(3.0) t0_ros = get_rostime() @@ -59,40 +59,72 @@ function main() pos_info_log.i = 1 # Start node - while t < 44.0 # run exactly 7 seconds + while t < 64.0 # run exactly x seconds t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 0 cmd.servo = 0 - elseif t <= 8 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.15 - elseif t <= 13 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.2 - elseif t <= 18 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.25 - elseif t <= 23 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.3 - elseif t <= 28 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.35 - elseif t <= 33 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.4 - elseif t <= 38 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.45 - elseif t <= 43 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = -0.5 - elseif t <= 44 - cmd.motor = -2.0 - cmd.servo = 0 + # SHIKANE: + # elseif t <= 3.5 + # cmd.motor = 2 + # cmd.servo = -0.3 + # elseif t <= 4 + # cmd.motor = 2 + # cmd.servo = 0.3 + # elseif t <= 4.5 + # cmd.motor = 2 + # cmd.servo = -0.3 + # elseif t <= 5 + # cmd.motor = 2 + # cmd.servo = 0.3 + # elseif t <= 5.5 + # cmd.motor = 2 + # cmd.servo = -0.3 + # elseif t <= 6 + # cmd.motor = 2 + # cmd.servo = 0.3 + # elseif t <= 6.5 + # cmd.motor = 2 + # cmd.servo = -0.3 + # elseif t <= 7 + # cmd.motor = 2 + # cmd.servo = 0.3 + # elseif t <= 7.5 + # cmd.motor = 2 + # cmd.servo = -0.3 + # CONTINUOUS ACCELERATION: + elseif t <= 63 + cmd.motor = 1.0#0.5+(t-3)/20 + cmd.servo = -(t-3.0)/300-0.1 + # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.15 + # elseif t <= 13 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.2 + # elseif t <= 18 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.25 + # elseif t <= 23 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.3 + # elseif t <= 28 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.35 + # elseif t <= 33 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.4 + # elseif t <= 38 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.45 + # elseif t <= 43 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = -0.5 + # elseif t <= 44 + # cmd.motor = -2.0 + # cmd.servo = 0 else - cmd.motor = 0 + cmd.motor = -2.0 cmd.servo = 0 end cmd.header.stamp = get_rostime() diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py index 4fee3c8a..e06ed404 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ b/workspace/src/barc/src/state_estimation_DynBkMdl.py @@ -19,128 +19,106 @@ from sensor_msgs.msg import Imu from marvelmind_nav.msg import hedge_pos from std_msgs.msg import Header -from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, size, sign -from numpy import unwrap, vstack, ones, linalg, polyval +from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, unwrap from observers import ekf from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_psi_drift, h_KinBkMdl_psi_drift from tf import transformations -# input variables -cmd_servo = 0 -cmd_motor = 0 -FxR = 0 - -# raw measurement variables -yaw_prev = 0 -yaw0 = 0 # yaw at t = 0 -yaw = 0 -(roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) -imu_times = [0]*25 -psiDot_hist = [0]*25 - -# Velocity -vel_est = 0 - -# GPS -x_meas = 0 -y_meas = 0 - -#x_hist = [0]*25 -#y_hist = [0]*25 -#gps_times = [0]*25 - -#poly_x = [0]*3 # polynomial coefficients for x and y position measurement (2nd order) -#poly_y = [0]*3 - -t0 = 0 -running = False - -# ecu command update -def ecu_callback(data): - global FxR, cmd_motor, cmd_servo, running - cmd_motor = data.motor # input motor force [N] - cmd_servo = data.servo # input steering angle [rad] - FxR = cmd_motor - if not running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero - running = True - -# ultrasound gps data -def gps_callback(data): - # units: [rad] and [rad/s] - global x_meas, y_meas, t0 - #current_t = rospy.get_rostime().to_sec() - x_meas = data.x_m - y_meas = data.y_m - #x_meas_pred = polyval(poly_x, data.header.stamp.to_sec()-t0) # predict new position - #y_meas_pred = polyval(poly_y, data.header.stamp.to_sec()-t0) - #x_meas_pred = polyval(poly_x, current_t-t0) - #y_meas_pred = polyval(poly_y, current_t-t0) - # if abs(x_meas_pred-data.x_m) < 0.5 and abs(y_meas_pred-data.y_m) < 0.5 or not running: # check for outlier - # x_meas = data.x_m # data is given in cm - # y_meas = data.y_m - - # x_hist.append(x_meas) - # y_hist.append(y_meas) - # #gps_times.append(data.header.stamp.to_sec()-t0) - # gps_times.append(current_t-t0) - # x_hist.pop(0) - # y_hist.pop(0) - # gps_times.pop(0) - -# imu measurement update -def imu_callback(data): - # units: [rad] and [rad/s] - global roll_meas, pitch_meas, yaw_meas, a_x, a_y, a_z, w_x, w_y, w_z - global yaw_prev, yaw0, yaw - global imu_times, psiDot_hist - current_t = rospy.get_rostime().to_sec() - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll_meas, pitch_meas, yaw_meas) = transformations.euler_from_quaternion(quaternion) - # yaw_meas is element of [-pi,pi] - yaw = unwrap([yaw_prev, yaw_meas])[1] # get smooth yaw (from beginning) - yaw_prev = yaw # and always use raw measured yaw for unwrapping - # from this point on 'yaw' will be definitely unwrapped (smooth)! - if not running: - yaw0 = yaw # set yaw0 to current yaw - yaw = 0 # and current yaw to zero - else: - yaw = yaw - yaw0 +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0 + cmd_motor = 0 + + # IMU + yaw_prev = 0 + yaw0 = 0 # yaw at t = 0 + yaw_meas = 0 + imu_times = [0]*25 + psiDot_hist = [0]*25 + + # Velocity + vel_meas = 0 + + # GPS + x_meas = 0 + y_meas = 0 + + # General variables + t0 = 0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + # units: [rad] and [rad/s] + #current_t = rospy.get_rostime().to_sec() + self.x_meas = data.x_m + self.y_meas = data.y_m + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 - #imu_times.append(data.header.stamp.to_sec()-t0) - imu_times.append(current_t-t0) - imu_times.pop(0) + #imu_times.append(data.header.stamp.to_sec()-t0) + self.imu_times.append(current_t-self.t0) + self.imu_times.pop(0) - # extract angular velocity and linear acceleration data - w_x = data.angular_velocity.x - w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - a_z = data.linear_acceleration.z + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + #a_x = data.linear_acceleration.x + #a_y = data.linear_acceleration.y + #a_z = data.linear_acceleration.z - psiDot_hist.append(w_z) - psiDot_hist.pop(0) + self.psiDot_hist.append(w_z) + self.psiDot_hist.pop(0) -def vel_est_callback(data): - global vel_est, t0, vel_est_hist, vel_times - if not data.vel_est == vel_est or not running: # if we're receiving a new measurement - vel_est = data.vel_est + def vel_est_callback(self, data): + if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement + self.vel_meas = data.vel_est # state estimation node def state_estimation(): - global t0, poly_x, poly_y + se = StateEst() # initialize node rospy.init_node('state_estimation', anonymous=True) # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, imu_callback) - rospy.Subscriber('vel_est', Vel_est, vel_est_callback) - rospy.Subscriber('ecu', ECU, ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, gps_callback) + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) # get vehicle dimension parameters @@ -158,7 +136,7 @@ def state_estimation(): loop_rate = 25 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) - t0 = rospy.get_rostime().to_sec() + se.t0 = rospy.get_rostime().to_sec() # settings about psi estimation (use different models accordingly) psi_drift_active = True @@ -195,21 +173,19 @@ def state_estimation(): d_f = 0 - x_pred = 0 - y_pred = 0 + # Estimation variables + (x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est, v_est, psi_drift_est) = [0]*8 + psi_dot_meas = 0 while not rospy.is_shutdown(): ros_t = rospy.get_rostime() - t = ros_t.to_sec()-t0 # current time + #t = ros_t.to_sec()-se.t0 # current time # calculate new steering angle (low pass filter on steering input to make v_y and v_x smoother) - d_f = d_f + (cmd_servo-d_f)*0.25 - #print d_f + d_f = d_f + (se.cmd_servo-d_f)*0.25 # GPS measurement update - x_meas_pred = x_meas - y_meas_pred = y_meas - sq_gps_dist = (x_meas-x_pred)**2 + (y_meas-y_pred)**2 + sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 # make R values dependent on current measurement R[0,0] = 1+10*sq_gps_dist R[1,1] = 1+10*sq_gps_dist @@ -219,28 +195,24 @@ def state_estimation(): #t_matrix_imu[0] = t_matrix_imu[0]**2 #poly_psiDot = linalg.lstsq(t_matrix_imu.T, psiDot_hist)[0] #psiDot_meas_pred = polyval(poly_psiDot, t) - psiDot_meas_pred = psiDot_hist[-1] + psi_dot_meas = se.psiDot_hist[-1] if psi_drift_active: - (x, y, psi, v, psi_drift) = z_EKF # note, r = EKF estimate yaw rate + (x_est, y_est, psi_est, v_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate else: - (x, y, psi, v) = z_EKF # note, r = EKF estimate yaw rate + (x_est, y_est, psi_est, v_est) = z_EKF # note, r = EKF estimate yaw rate # use Kalman values to predict state in 0.1s - dt_pred = 0.0 - bta = arctan(L_f/(L_f+L_r)*tan(d_f)) - x_pred = x# + dt_pred*(v*cos(psi + bta)) - y_pred = y# + dt_pred*(v*sin(psi + bta)) - psi_pred = psi# + dt_pred*v/L_r*sin(bta) - v_pred = v# + dt_pred*(FxR - 0.63*sign(v)*v**2) - v_x_pred = cos(bta)*v_pred - v_y_pred = sin(bta)*v_pred - - psi_dot_pred = psiDot_meas_pred + #x_pred = x# + dt_pred*(v*cos(psi + bta)) + #y_pred = y# + dt_pred*(v*sin(psi + bta)) + #psi_pred = psi# + dt_pred*v/L_r*sin(bta) + #v_pred = v# + dt_pred*(FxR - 0.63*sign(v)*v**2) + v_x_est = cos(bta)*v_est + v_y_est = sin(bta)*v_est # Update track position - l.set_pos(x_pred, y_pred, psi_pred, v_x_pred, v_x_pred, v_y_pred, psi_dot_pred) # v = v_x + l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) # v = v_x #l.find_s() l.s = 0 l.epsi = 0 @@ -248,25 +220,23 @@ def state_estimation(): # and then publish position info state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, x_meas_pred, y_meas_pred, yaw, vel_est, psi_drift, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, l.coeffX.tolist(), l.coeffY.tolist(), l.coeffTheta.tolist(), l.coeffCurvature.tolist())) - - # apply EKF # get measurement if est_mode == 1: - y = array([x_meas_pred, y_meas_pred, yaw, vel_est]) + y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas]) elif est_mode == 2: - y = array([yaw, vel_est]) + y = array([se.yaw_meas, se.vel_meas]) elif est_mode == 3: - y = array([x_meas, y_meas]) + y = array([se.x_meas, se.y_meas]) elif est_mode == 4: - y = array([x_meas, y_meas, vel_est]) + y = array([se.x_meas, se.y_meas, se.vel_meas]) else: print "Wrong estimation mode specified." # define input - u = array([d_f, FxR]) + u = array([d_f, se.cmd_motor]) # build extra arguments for non-linear function args = (u, vhMdl, dt, est_mode) diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py new file mode 100755 index 00000000..b644d477 --- /dev/null +++ b/workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, unwrap +from observers import ekf +from system_models import f_KinBkMdl_mixed, h_KinBkMdl_mixed +from tf import transformations + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0 + cmd_motor = 0 + + # IMU + yaw_prev = 0 + yaw0 = 0 # yaw at t = 0 + yaw_meas = 0 + psiDot_meas = 0 + a_x_meas = 0 + a_y_meas = 0 + + # Velocity + vel_meas = 0 + + # GPS + x_meas = 0 + y_meas = 0 + + # General variables + t0 = 0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + # units: [rad] and [rad/s] + #current_t = rospy.get_rostime().to_sec() + self.x_meas = data.x_m + self.y_meas = data.y_m + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 + + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + #a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + self.a_x_meas = a_x + self.a_y_meas = a_y + + def vel_est_callback(self, data): + if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement + self.vel_meas = data.vel_est + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # get EKF observer properties + psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise + v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation + gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements + est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode + + # set node rate + loop_rate = 25 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() + + z_EKF = zeros(11) # x, y, psi, v, psi_drift + P = eye(11) # initial dynamics coveriance matrix + + Q = diag([0.01,0.01,1/2*dt**2,1/2*dt**2,0.001,0.001,0.01,0.01,0.00001,0.00001,0.00001]) + R = diag([0.1,0.1,0.1,0.1,0.01,1.0,1.0]) + + # Set up track parameters + l = Localization() + l.create_track() + l.prepare_trajectory(0.06) + + d_f = 0 + + # Estimation variables + (x_est, y_est) = [0]*2 + + while not rospy.is_shutdown(): + # make R values dependent on current measurement (robust against outliers) + sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 + if sq_gps_dist > 0.2: + R[0,0] = 1+10*sq_gps_dist**2 + R[1,1] = 1+10*sq_gps_dist**2 + else: + R[0,0] = 0.1 + R[1,1] = 0.1 + + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas]) + + # define input + u = [0,0] + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, est_mode) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_KinBkMdl_mixed, z_EKF, P, h_KinBkMdl_mixed, y, Q, R, args) + + # Read values + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, a_x_drift_est, a_y_drift_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate + + # Update track position + l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) # v = v_x + l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, + l.coeffX.tolist(), l.coeffY.tolist(), + l.coeffTheta.tolist(), l.coeffCurvature.tolist())) + + # wait + rate.sleep() + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py b/workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py new file mode 100755 index 00000000..82aea247 --- /dev/null +++ b/workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap +from observers import ekf +from system_models import f_KinBkMdl_mixed_nodrift, h_KinBkMdl_mixed_nodrift +from tf import transformations + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0 + cmd_motor = 0 + + # IMU + yaw_prev = 0 + yaw0 = 0 # yaw at t = 0 + yaw_meas = 0 + psiDot_meas = 0 + a_x_meas = 0 + a_y_meas = 0 + + # Velocity + vel_meas = 0 + + # GPS + x_meas = 0 + y_meas = 0 + + # General variables + t0 = 0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + # units: [rad] and [rad/s] + #current_t = rospy.get_rostime().to_sec() + self.x_meas = data.x_m + self.y_meas = data.y_m + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 + + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + #a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + self.a_x_meas = a_x + self.a_y_meas = a_y + + def vel_est_callback(self, data): + if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement + self.vel_meas = data.vel_est + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # get EKF observer properties + psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise + v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation + gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements + est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() + + z_EKF = zeros(9) # x, y, psi, v, psi_drift + P = eye(9) # initial dynamics coveriance matrix + + Q = diag([0.01,0.01,0.1,0.1,0.1,0.1,0.01,0.1,0.0000001]) + R = diag([0.1,0.1,0.1,0.1,0.01,1.0,1.0]) + + # Set up track parameters + l = Localization() + l.create_track() + l.prepare_trajectory(0.06) + + d_f = 0 + + # Estimation variables + (x_est, y_est) = [0]*2 + + while not rospy.is_shutdown(): + # make R values dependent on current measurement (robust against outliers) + sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 + if sq_gps_dist > 0.2: + R[0,0] = 1+10*sq_gps_dist**2 + R[1,1] = 1+10*sq_gps_dist**2 + else: + R[0,0] = 0.1 + R[1,1] = 0.1 + + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas]) + + # define input + u = [0,0] + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, est_mode) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_KinBkMdl_mixed_nodrift, z_EKF, P, h_KinBkMdl_mixed_nodrift, y, Q, R, args) + + # Read values + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate + + # Update track position + l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) # v = v_x + #l.find_s() + l.s = 0 + l.epsi = 0 + l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, + l.coeffX.tolist(), l.coeffY.tolist(), + l.coeffTheta.tolist(), l.coeffCurvature.tolist())) + + # wait + rate.sleep() + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 0f92c26e..b560582f 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -1,23 +1,24 @@ #!/usr/bin/env python +# -*- coding: utf-8 -*- # --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for +# Licensing Information: You are free to use or extend these projects for # education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link +# and (2) you provide clear attribution to UC Berkeley, including a link # to http://barc-project.com # # Attibution Information: The barc project ROS code-base was developed # at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales # (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was # based on an open source project by Bruce Wootton # --------------------------------------------------------------------------- from numpy import sin, cos, tan, arctan, array, dot -from numpy import sign, argmin, sqrt -import rospy +from numpy import sign, sqrt +import math -def f_KinBkMdl(z,u,vhMdl, dt, est_mode): +def f_KinBkMdl(z, u, vhMdl, dt, est_mode): """ process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] @@ -48,7 +49,7 @@ def f_KinBkMdl(z,u,vhMdl, dt, est_mode): return array([x_next, y_next, psi_next, v_next]) -def f_KinBkMdl_psi_drift(z,u,vhMdl, dt, est_mode): +def f_KinBkMdl_psi_drift(z, u, vhMdl, dt, est_mode): """ process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] @@ -85,7 +86,7 @@ def f_KinBkMdl_psi_drift(z,u,vhMdl, dt, est_mode): return array([x_next, y_next, psi_next, v_next, psi_drift_next]) -def f_KinBkMdl_predictive(z,u,vhMdl, dt, est_mode): +def f_KinBkMdl_predictive(z, u, vhMdl, dt, est_mode): """ process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] @@ -108,7 +109,7 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt, est_mode): a = u[1] # extract parameters - (L_a, L_b) = vhMdl + (L_a, L_b) = vhMdl # compute slip angle bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) @@ -127,9 +128,71 @@ def f_KinBkMdl_predictive(z,u,vhMdl, dt, est_mode): return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) + +def f_KinBkMdl_mixed(z, u, vhMdl, dt, est_mode): + (l_A,l_B) = vhMdl + #bta = atan(l_A/(l_A+l_B)*tan(u[2])) + bta = math.atan2(z[3],z[2]) + zNext = [0]*11 + zNext[0] = z[0] + dt*(sqrt(z[2]**2+z[3]**2)*cos(z[6] + bta)) # x + zNext[1] = z[1] + dt*(sqrt(z[2]**2+z[3]**2)*sin(z[6] + bta)) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(sqrt(z[2]**2+z[3]**2)/l_B*sin(bta)) # psi + zNext[7] = z[7] # psidot + zNext[8] = z[8] # drift_a_x + zNext[9] = z[9] # drift_a_y + zNext[10] = z[10] # drift_psi + return array(zNext) + +def f_KinBkMdl_mixed_nodrift(z, u, vhMdl, dt, est_mode): + (l_A,l_B) = vhMdl + #bta = atan(l_A/(l_A+l_B)*tan(u[2])) + bta = math.atan2(z[3],z[2]) + #print "========================" + #print "bta = %f"%bta + #print "psi = %f"%z[6] + zNext = [0]*9 + zNext[0] = z[0] + dt*(sqrt(z[2]**2+z[3]**2)*cos(z[6] + bta)) # x + zNext[1] = z[1] + dt*(sqrt(z[2]**2+z[3]**2)*sin(z[6] + bta)) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(sqrt(z[2]**2+z[3]**2)/l_B*sin(bta)) # psi + zNext[7] = z[7] # psidot + zNext[8] = z[8] # drift_psi + return array(zNext) + +def h_KinBkMdl_mixed(x, u, vhMdl, dt, est_mode): + """This Measurement model uses gps, imu and encoders""" + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = sqrt(x[2]**2+x[3]**2) # v + y[3] = x[6]+x[10] # psi + y[4] = x[7] # psiDot + y[5] = x[4]+x[8] # a_x + y[6] = x[5]+x[9] # a_y + return array(y) + +def h_KinBkMdl_mixed_nodrift(x, u, vhMdl, dt, est_mode): + """This Measurement model uses gps, imu and encoders""" + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = sqrt(x[2]**2+x[3]**2) # v + y[3] = x[6]+x[8] # psi + y[4] = x[7] # psiDot + y[5] = x[4] # a_x + y[6] = x[5] # a_y + return array(y) + def h_KinBkMdl(x, u, vhMdl, dt, est_mode): """ - measurement model + Measurement model """ if est_mode==1: # GPS, IMU, Enc C = array([[1, 0, 0, 0], @@ -152,7 +215,7 @@ def h_KinBkMdl(x, u, vhMdl, dt, est_mode): def h_KinBkMdl_psi_drift(x, u, vhMdl, dt, est_mode): """ - measurement model + Measurement model """ if est_mode==1: # GPS, IMU, Enc C = array([[1, 0, 0, 0, 0], From 98f01230777e4f4f7d827aebe2dfd7173303c35a Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 3 Nov 2016 09:50:55 -0700 Subject: [PATCH 080/183] Added 4-wheel encoder logging, added simpler kinematic estimator (not using IMU accelerations) --- .../arduino_nano328_node.ino | 10 +- .../{ => Kalman simulation}/sim_kalman.jl | 0 .../{ => Kalman simulation}/sim_kalman_imu.jl | 0 .../{ => Kalman simulation}/sim_kalman_mix.jl | 0 .../Kalman simulation/sim_kalman_mix_2.jl | 373 ++++++++++++++++++ .../sim_kalman_mix_nodrift.jl | 0 .../find_d_f_delay.jl | 0 .../find_d_f_mapping.jl | 0 .../Parameter estimation/find_u_a_drag.jl | 33 ++ workspace/src/barc/src/barc_record.jl | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 4 + workspace/src/barc/src/open_loop.jl | 2 +- .../src/state_estimation_KinBkMdl_mixed.py | 207 ++++++++++ workspace/src/barc/src/system_models.py | 26 ++ 14 files changed, 650 insertions(+), 7 deletions(-) rename eval_sim/{ => Kalman simulation}/sim_kalman.jl (100%) rename eval_sim/{ => Kalman simulation}/sim_kalman_imu.jl (100%) rename eval_sim/{ => Kalman simulation}/sim_kalman_mix.jl (100%) create mode 100644 eval_sim/Kalman simulation/sim_kalman_mix_2.jl rename eval_sim/{ => Kalman simulation}/sim_kalman_mix_nodrift.jl (100%) rename eval_sim/{ => Parameter estimation}/find_d_f_delay.jl (100%) rename eval_sim/{ => Parameter estimation}/find_d_f_mapping.jl (100%) create mode 100644 eval_sim/Parameter estimation/find_u_a_drag.jl create mode 100755 workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index ad0ac7e1..96adcb06 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -61,6 +61,11 @@ class Car { void calcThrottle(); void calcSteering(); float getVelocityEstimate(); + float vel_FL = 0; + float vel_FR = 0; + float vel_BL = 0; + float vel_BR = 0; + private: // Pin assignments const int ENC_FL_PIN = 2; @@ -128,11 +133,6 @@ class Car { int BL_count = 0; int BR_count = 0; - float vel_FL = 0; - float vel_FR = 0; - float vel_BL = 0; - float vel_BR = 0; - // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) volatile unsigned long FL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) diff --git a/eval_sim/sim_kalman.jl b/eval_sim/Kalman simulation/sim_kalman.jl similarity index 100% rename from eval_sim/sim_kalman.jl rename to eval_sim/Kalman simulation/sim_kalman.jl diff --git a/eval_sim/sim_kalman_imu.jl b/eval_sim/Kalman simulation/sim_kalman_imu.jl similarity index 100% rename from eval_sim/sim_kalman_imu.jl rename to eval_sim/Kalman simulation/sim_kalman_imu.jl diff --git a/eval_sim/sim_kalman_mix.jl b/eval_sim/Kalman simulation/sim_kalman_mix.jl similarity index 100% rename from eval_sim/sim_kalman_mix.jl rename to eval_sim/Kalman simulation/sim_kalman_mix.jl diff --git a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl new file mode 100644 index 00000000..ffabc5a7 --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl @@ -0,0 +1,373 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) + +# pos_info[1] = s +# pos_info[2] = eY +# pos_info[3] = ePsi +# pos_info[4] = v +# pos_info[5] = s_start +# pos_info[6] = x +# pos_info[7] = y +# pos_info[8] = v_x +# pos_info[9] = v_y +# pos_info[10] = psi +# pos_info[11] = psiDot +# pos_info[12] = x_raw +# pos_info[13] = y_raw +# pos_info[14] = psi_raw +# pos_info[15] = v_raw +# pos_info[16] = psi_drift + + +function main(code::AbstractString) + global Q, R, R_gps_imu + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1])+0.3 + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end])-0.3 + + l_A = 0.125 + l_B = 0.125 + + dt = 1/50 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,5) + + x_est = zeros(length(t),6) + P_gps_imu = zeros(6,6) + x_est_gps_imu = zeros(length(t),8) + #x_est_gps_imu[1,11] = 0.5 + #x_est_gps_imu[1,10] = -0.6 + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + + Q_gps_imu = diagm([1.0,1.0,1.0,0.1,0.1,0.01]) + R_gps_imu = diagm([1.0,1.0,0.1,0.1,5.0]) + + gps_gate = zeros(length(t)) + + gps_prev = gps_meas.z[1,:] + # measured: IMU_a + # calibrated: Z_a + # need matrix Z_IMU_A + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] + y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + y_vel_est = vel_est.z[t[i].>vel_est.t][end] + + y_gps_imu[i,:] = [y_gps y_yaw y_vel_est y_yawdot] + y_gps_imu[:,3] = unwrap!(y_gps_imu[:,3]) + + u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] + u[i,2] = cmd_log.z[t[i].>cmd_log.t-0.2,2][end] + + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + #gps_gate[i] = + # if gps_dist[i] > 1.0 + # R[1,1] = 1+10*gps_dist[i]^2 + # R[2,2] = 1+10*gps_dist[i]^2 + # R_gps_imu[1,1] = 1.0+100*gps_dist[i]^2 + # R_gps_imu[2,2] = 1.0+100*gps_dist[i]^2 + # else + # R[1,1] = 1 + # R[2,2] = 1 + # R_gps_imu[1,1] = 0.1 + # R_gps_imu[2,2] = 0.1 + # end + if gps_prev == y_gps + R_gps_imu[1:2,1:2] = 10.0*eye(2) + else + R_gps_imu[1:2,1:2] = 1.0*eye(2) + end + gps_prev = y_gps + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est_gps_imu[i,1:6], P_gps_imu, gps_gate[i]) = ekf_gate(simModel_gps_imu,x_est_gps_imu[i-1,1:6]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + beta = atan(l_A*tan(u[i,2])/(l_B+l_A)) + x_est_gps_imu[i,7] = cos(beta)*x_est_gps_imu[i,4] # v_x + x_est_gps_imu[i,8] = l_B*x_est_gps_imu[i,5] # v_y + end + + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-x",pos_info.t-t0,pos_info.z[:,6:7],"--") + grid("on") + title("Comparison x,y estimate and measurement") + legend(["x_est","y_est","x_meas","y_meas","x_onboard","y_onboard"]) + + figure(2) + plot(t-t0,x_est_gps_imu[:,6]) + grid("on") + title("Drifts") + legend(["psi"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z,pos_info.t-t0,pos_info.z[:,8:9],"-x") + grid("on") + legend(["v","v_x_est","v_y_est","v_meas","v_x_onboard","v_y_onboard"]) + title("Velocity estimate and measurement") + + figure(4) + plot(t-t0,x_est_gps_imu[:,3],"-*",t-t0,y_gps_imu[:,3]) + grid("on") + title("Comparison yaw") + legend(["psi_est","psi_meas"]) + + figure(5) + plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,5]) + title("w_z") + legend(["w_z_meas","w_z_est"]) + grid("on") + + figure(6) + plot(t-t0,x_est_gps_imu[:,7:8]) + grid("on") + legend(["v_x_est","v_y_est"]) + + figure(7) + plot(t-t0,gps_gate,t-t0,gps_dist,gps_meas.t-t0,gps_meas.z) + grid("on") + legend(["gate","dist"]) + + # # CORRELATIONS: + # figure(8) + # plot(94.14+2.7678*u[:,1],y[:,4],"*") + # grid("on") + # title("Comparison motor input and velocity") + # xlabel("PWM signal") + # ylabel("v [m/s]") + + # delta = atan(x_est_gps_imu[:,8]*0.25./x_est_gps_imu[:,3]) + # #delta2 = atan(0.25/sqrt(y_gps_imu[:,3].^2./y_gps_imu[:,5].^2-0.125^2)) + # figure(9) + # plot(u[:,2],delta,"*") + # grid("on") + + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + # figure(1) + # ax=subplot(5,1,1) + # for i=1:4 + # subplot(5,1,i,sharex=ax) + # plot(t,y[:,i],t,x_est[:,i],"-*") + # grid("on") + # end + # subplot(5,1,5,sharex=ax) + # plot(cmd_pwm_log.t,cmd_pwm_log.z) + # grid("on") + + + # figure(2) + # subplot(2,1,1) + # title("Comparison raw GPS data and estimate") + # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["x_m","x_est"]) + # grid("on") + # subplot(2,1,2) + # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["y_m","y_est"]) + # grid("on") + + # figure(3) + # plot(t,gps_dist) + # title("GPS distances") + # grid("on") + # # figure() + # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # # grid("on") + # # title("x-y-view") + # figure(4) + # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + # title("Comparison simulation (-x) onboard-estimate (-*)") + # grid("on") + # legend(["x","y","psi","v","psi_drift"]) + + # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + # figure(5) + # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + # title("Comparison yaw") + # grid("on") + + # figure(6) + # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + # grid("on") + # title("Comparison of velocity measurement and estimate") + # legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h_gps_imu(x,args) + y = zeros(5) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = x[3]+x[6] # psi + y[4] = x[4] # v + y[5] = x[5] # psiDot + return y +end + +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function ekf_gate(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + S = H*P12 + gps_err = ((y_kp1[1:2]-my_kp1[1:2])'*inv(S[1:2,1:2])*(y_kp1[1:2]-my_kp1[1:2]))[1] + # if gps_err > 100.0 + # R[1:2,1:2] = eye(2)*100 + # else + # R[1:2,1:2] = eye(2)*0.1 + # end + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1, gps_err +end + +function simModel_gps_imu(z,args) + (u,dt,l_A,l_B) = args + #bta = atan(l_A/(l_A+l_B)*tan(u[2])) + bta = atan2(l_A*tan(u[2]),l_A+l_B) + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1]-1.0*z[4]) # v + zNext[5] = z[5] # psiDot + zNext[6] = z[6] # drift_psi + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end + +function angRates2EulerRates(w::Array{Float64},deg::Array{Float64}) + (p,q,r) = w + (phi,theta,psi) = deg + phidot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r + thetadot = cos(phi)*q - sin(phi)*r + psidot = sin(phi)*sec(theta)*q + cos(phi)*sec(theta)*r + return [phidot,thetadot,psidot] +end + +function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body + return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) +end \ No newline at end of file diff --git a/eval_sim/sim_kalman_mix_nodrift.jl b/eval_sim/Kalman simulation/sim_kalman_mix_nodrift.jl similarity index 100% rename from eval_sim/sim_kalman_mix_nodrift.jl rename to eval_sim/Kalman simulation/sim_kalman_mix_nodrift.jl diff --git a/eval_sim/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl similarity index 100% rename from eval_sim/find_d_f_delay.jl rename to eval_sim/Parameter estimation/find_d_f_delay.jl diff --git a/eval_sim/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl similarity index 100% rename from eval_sim/find_d_f_mapping.jl rename to eval_sim/Parameter estimation/find_d_f_mapping.jl diff --git a/eval_sim/Parameter estimation/find_u_a_drag.jl b/eval_sim/Parameter estimation/find_u_a_drag.jl new file mode 100644 index 00000000..c11574b8 --- /dev/null +++ b/eval_sim/Parameter estimation/find_u_a_drag.jl @@ -0,0 +1,33 @@ + +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_log.z[t[i].>cmd_log.t,1][end] > 0 && vel_est.z[t[i].>vel_est.t][end] > 0.2 + v[i] = vel_est.z[t[i].>vel_est.t][end] + cmd[i] = cmd_log.z[t[i].>cmd_log.t,1][end] + end + end + #plot(cmd,v) + plot(cmd./v,"*") + grid("on") +end \ No newline at end of file diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index f1cf88ed..7af3cc0f 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -33,7 +33,7 @@ function main() imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) - vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) + vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,16)) # initiate node, set up publisher / subscriber topics diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 886245a2..37510b5f 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -153,6 +153,10 @@ function main() if sum(dist_traveled .>= vel_dist_update)>=1 && z_current[i,3] > 0.1 # only update if at least one of the magnets has passed the sensor dist_traveled[dist_traveled.>=vel_dist_update] = 0 vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) + vel_est.vel_fl = 0 + vel_est.vel_fr = 0 + vel_est.vel_bl = 0 + vel_est.vel_br = 0 end vel_est.header.stamp = t_ros publish(pub_vel, vel_est) diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index d7f42aa7..e4e8a005 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -24,7 +24,7 @@ function main() imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) - vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize)) + vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,16)) # Initialize ROS node and topics diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py new file mode 100755 index 00000000..35b9cffb --- /dev/null +++ b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos +from observers import ekf +from system_models import f_KinBkMdl_2, h_KinBkMdl_2 +from tf import transformations +import math + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0 + cmd_motor = 0 + cmd_t = 0 + + # IMU + yaw_prev = 0 + yaw0 = 0 # yaw at t = 0 + yaw_meas = 0 + psiDot_meas = 0 + a_x_meas = 0 + a_y_meas = 0 + + # Velocity + vel_meas = 0 + + # GPS + x_meas = 0 + y_meas = 0 + + # General variables + t0 = 0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + # units: [rad] and [rad/s] + #current_t = rospy.get_rostime().to_sec() + self.x_meas = data.x_m + self.y_meas = data.y_m + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 + + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + #a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + self.a_x_meas = a_x + self.a_y_meas = a_y + + def vel_est_callback(self, data): + if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement + self.vel_meas = data.vel_est + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # get EKF observer properties + psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise + v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation + gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements + est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() + + z_EKF = zeros(6) # x, y, psi, v, psi_drift + P = eye(6) # initial dynamics coveriance matrix + + Q = diag([1.0,1.0,1.0,0.1,0.1,0.01]) + R = diag([1.0,1.0,0.1,0.1,5.0]) + + # Set up track parameters + l = Localization() + l.create_track() + l.prepare_trajectory(0.06) + + d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + + # Estimation variables + (x_est, y_est) = [0]*2 + + while not rospy.is_shutdown(): + # make R values dependent on current measurement (robust against outliers) + sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 + if sq_gps_dist > 0.5: + R[0,0] = 1+10*sq_gps_dist**2 + R[1,1] = 1+10*sq_gps_dist**2 + else: + R[0,0] = 0.1 + R[1,1] = 0.1 + + # get measurement + y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas]) + + # define input + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of the steering + u = [se.cmd_motor, d_f_hist.pop(0)] + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, est_mode) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_KinBkMdl_2, z_EKF, P, h_KinBkMdl_2, y, Q, R, args) + + # Read values + (x_est, y_est, psi_est, v_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate + bta = math.atan2(L_f*tan(u[1]),L_f+L_r) + v_y_est = L_r*psi_dot_est + v_x_est = cos(bta)*v_est + + # Update track position + l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) + #l.find_s() + l.s = 0 + l.epsi = 0 + l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, + l.coeffX.tolist(), l.coeffY.tolist(), + l.coeffTheta.tolist(), l.coeffCurvature.tolist())) + + # wait + rate.sleep() + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index b560582f..83a4cc39 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -166,6 +166,32 @@ def f_KinBkMdl_mixed_nodrift(z, u, vhMdl, dt, est_mode): zNext[8] = z[8] # drift_psi return array(zNext) +def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): + (l_A,l_B) = vhMdl + #bta = atan(l_A/(l_A+l_B)*tan(u[2])) + bta = math.atan2(l_A*tan(u[1]),l_A+l_B) + #print "========================" + #print "bta = %f"%bta + #print "psi = %f"%z[6] + zNext = [0]*4 + zNext[0] = z[0] + dt*(z[3]*cos(z[2] + bta)) # x + zNext[1] = z[1] + dt*(z[3]*sin(z[2] + bta)) # y + zNext[2] = z[2] + dt*(z[3]/l_B*sin(bta)) # psi + zNext[3] = z[3] + dt*(u[0] - 1.0*z[3]) # v + zNext[4] = z[4] # psiDot + zNext[5] = z[5] # drift_psi + return array(zNext) + +def h_KinBkMdl_2(x, u, vhMdl, dt, est_mode): + """This Measurement model uses gps, imu and encoders""" + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2]+x[5] # psi + y[3] = x[3] # v + y[4] = x[4] # psiDot + return array(y) + def h_KinBkMdl_mixed(x, u, vhMdl, dt, est_mode): """This Measurement model uses gps, imu and encoders""" y = [0]*7 From fb0d6bc66c761b0cc4aa6ae5d3226fcf40b038d2 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 4 Nov 2016 09:28:54 -0700 Subject: [PATCH 081/183] Fixed arduino code, extended evaluation scripts --- .../arduino_nano328_node.ino | 58 ++++++------ .../Kalman simulation/sim_kalman_mix_2.jl | 32 ++++++- .../Parameter estimation/find_d_f_delay.jl | 9 +- .../Parameter estimation/find_d_f_mapping.jl | 91 ++++++++++++++++--- .../Parameter estimation/find_u_a_drag.jl | 22 ++++- workspace/src/barc/launch/barc_sim.launch | 2 +- .../src/barc/launch/open_loop_remote.launch | 2 +- .../src/barc/launch/run_lmpc_remote.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 6 +- workspace/src/barc/src/barc_simulator_dyn.jl | 17 ++-- .../src/barc/src/controller_low_level.py | 23 +++-- workspace/src/barc/src/open_loop.jl | 62 +++++++------ .../src/state_estimation_KinBkMdl_mixed.py | 49 +++++++--- workspace/src/barc/src/system_models.py | 4 +- 14 files changed, 268 insertions(+), 111 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index 96adcb06..05b47a1b 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -79,13 +79,13 @@ class Car { // Car properties // unclear what this is for - const int noAction = 0; + const float noAction = 0.0; // Motor limits // TODO are these the real limits? - const int MOTOR_MAX = 120; - const int MOTOR_MIN = 40; - const int MOTOR_NEUTRAL = 90; + const float MOTOR_MAX = 120.0; + const float MOTOR_MIN = 40.0; + const float MOTOR_NEUTRAL = 90.0; // Optional: smaller values for testing safety /* const int MOTOR_MAX = 100; */ /* const int MOTOR_MIN = 75; */ @@ -95,10 +95,10 @@ class Car { // 120] judging from the sound of the servo pushing beyond a mechanical limit // outside that range. The offset may be 2 or 3 deg and the d_theta_max is then // ~31. - const int D_THETA_MAX = 30; - const int THETA_CENTER = 90; - const int THETA_MAX = THETA_CENTER + D_THETA_MAX; - const int THETA_MIN = THETA_CENTER - D_THETA_MAX; + const float D_THETA_MAX = 30.0; + const float THETA_CENTER = 90.0; + const float THETA_MAX = THETA_CENTER + D_THETA_MAX; + const float THETA_MIN = THETA_CENTER - D_THETA_MAX; // Interfaces to motor and steering actuators Servo motor; @@ -208,7 +208,7 @@ void setup() { // Set up encoders, rc input, and actuators car.initEncoders(); - car.initRCInput(); + //car.initRCInput(); car.initActuators(); // Start ROS node @@ -317,7 +317,7 @@ void Car::armActuators() { void Car::writeToActuators(const barc::ECU& ecu) { float motorCMD = saturateMotor(ecu.motor); float servoCMD = saturateServo(ecu.servo); - motor.write(motorCMD); + motor.writeMicroseconds( (uint16_t) (1500 + (motorCMD-90.0)*1000.0/180.0)) steering.write(servoCMD); } @@ -351,28 +351,28 @@ void Car::calcSteering() { void Car::incFL() { FL_count_shared++; FL_old_time = FL_new_time; //(ADDED BY TOMMI 7JULY2016) - FL_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) + FL_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FL_FLAG; } void Car::incFR() { FR_count_shared++; FR_old_time = FR_new_time; //(ADDED BY TOMMI 7JULY2016) - FR_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) + FR_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FR_FLAG; } void Car::incBL() { BL_count_shared++; BL_old_time = BL_new_time; //(ADDED BY TOMMI 7JULY2016) - BL_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) + BL_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BL_FLAG; } void Car::incBR() { BR_count_shared++; BR_old_time = BR_new_time; //(ADDED BY TOMMI 7JULY2016) - BR_new_time = micros(); // new istant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) + BR_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BR_FLAG; } @@ -434,7 +434,6 @@ int Car::getEncoderBR() { return BR_count; } - unsigned long Car::getEncoder_dTime_FL() { //(ADDED BY TOMMI 7JULY2016) return FL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) } //(ADDED BY TOMMI 7JULY2016) @@ -449,23 +448,22 @@ unsigned long Car::getEncoder_dTime_BR() { //(ADDE } //(ADDED BY TOMMI 7JULY2016) float Car::getVelocityEstimate() { - if(FL_DeltaTime > 0){ - vel_FL = 2.0*3.141593*0.036/2.0*1.0/FL_DeltaTime; - } - if(FR_DeltaTime > 0){ - vel_FR = 2.0*3.141593*0.036/2.0*1.0/FR_DeltaTime; + float t_min = 2.0 // minimum time (in s) when v is set to zero, this correlates to a minimum velocity of 0.05 m/s + vel_FL = 0.0 + vel_FR = 0.0 + vel_BL = 0.0 + vel_BR = 0.0 + if(FL_DeltaTime < t_min){ + vel_FL = 2.0*3.141593*0.036/2.0*1.0/FL_DeltaTime*1000000.0; } - if(BL_DeltaTime > 0){ - vel_BL = 2.0*3.141593*0.036/2.0*1.0/BL_DeltaTime; + if(FR_DeltaTime < t_min){ + vel_FR = 2.0*3.141593*0.036/2.0*1.0/FR_DeltaTime*1000000.0; } - if(BR_DeltaTime > 0){ - vel_BR = 2.0*3.141593*0.036/2.0*1.0/BR_DeltaTime; + if(BL_DeltaTime < t_min){ + vel_BL = 2.0*3.141593*0.036/2.0*1.0/BL_DeltaTime*1000000.0; } - if(FL_DeltaTime > 0 && FR_DeltaTime > 0 && BR_DeltaTime) { - return 2.0*3.141593*0.036/2.0*(1.0/FL_DeltaTime + 1.0/FR_DeltaTime + 1.0/BR_DeltaTime)*1000000.0/3.0; // calculate current speed in m/s - //return 1.0; -} - else { - return 0.0; + if(BR_DeltaTime < t_min){ + vel_BR = 2.0*3.141593*0.036/2.0*1.0/BR_DeltaTime*1000000.0; } + return ( vel_FL + vel_FR ) / 2.0 } diff --git a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl index ffabc5a7..9480d265 100644 --- a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl +++ b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl @@ -68,6 +68,8 @@ function main(code::AbstractString) gps_gate = zeros(length(t)) gps_prev = gps_meas.z[1,:] + vel_est_prev = vel_est.z[1,1] + imu_prev = imu_meas.z[1,6] # measured: IMU_a # calibrated: Z_a # need matrix Z_IMU_A @@ -78,7 +80,7 @@ function main(code::AbstractString) y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] a_x = imu_meas.z[t[i].>imu_meas.t,7][end] a_y = imu_meas.z[t[i].>imu_meas.t,8][end] - y_vel_est = vel_est.z[t[i].>vel_est.t][end] + y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] y_gps_imu[i,:] = [y_gps y_yaw y_vel_est y_yawdot] y_gps_imu[:,3] = unwrap!(y_gps_imu[:,3]) @@ -100,11 +102,25 @@ function main(code::AbstractString) # R_gps_imu[2,2] = 0.1 # end if gps_prev == y_gps + R_gps_imu[1:2,1:2] = 100.0*eye(2) + else R_gps_imu[1:2,1:2] = 10.0*eye(2) + end + if vel_est_prev == y_vel_est + R_gps_imu[4,4] = 10.0 else - R_gps_imu[1:2,1:2] = 1.0*eye(2) + R_gps_imu[4,4] = 0.1 + end + if imu_prev == y_yaw + R_gps_imu[3,3] = 100000.0 + R_gps_imu[5,5] = 10.0 + else + R_gps_imu[3,3] = 0.1 + R_gps_imu[5,5] = 5.0 end gps_prev = y_gps + vel_est_prev = y_vel_est + imu_prev = y_yaw args = (u[i,:],dt,l_A,l_B) # Calculate new estimate @@ -370,4 +386,16 @@ end function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) +end + +function getIndex(t_raw::Array{Float64},t::Float64) + i = 1 + while i<=length(t_raw) + if t > t_raw[i] + i += 1 + else + break + end + end + return i end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl index 4af5a99a..e3cf1025 100644 --- a/eval_sim/Parameter estimation/find_d_f_delay.jl +++ b/eval_sim/Parameter estimation/find_d_f_delay.jl @@ -22,22 +22,25 @@ function main(code::AbstractString) psiDot = zeros(length(t)) for i=1:length(t) - v[i] = vel_est.z[t[i].>vel_est.t][end] + v[i] = vel_est.z[t[i].>vel_est.t,1][end] psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] end v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) v_y = L_b*psiDot delta = atan2(psiDot*0.25,v_x) + figure(1) plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) grid("on") xlabel("t [s]") legend(["delta_true","delta_input"]) + figure(2) ax1=subplot(211) - plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,1])) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) grid("on") subplot(212,sharex=ax1) - plot(vel_est.t-t0,vel_est.z) + plot(vel_est.t-t0,vel_est.z,vel_est.t-t0,mean(vel_est.z[:,2:3],2,),"--") grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br","v_mean"]) end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl index 85de5664..bc22d70a 100644 --- a/eval_sim/Parameter estimation/find_d_f_mapping.jl +++ b/eval_sim/Parameter estimation/find_d_f_mapping.jl @@ -2,15 +2,84 @@ using PyPlot using JLD +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + function main() - log_path_record_1 = "$(homedir())/open_loop/output-record-8bbb.jld" - log_path_record_2 = "$(homedir())/open_loop/output-record-3593.jld" - d_rec_1 = load(log_path_record) - d_rec_2 = load(log_path_record) - - imu_meas = d_rec["imu_meas"] - gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - cmd_pwm_log = d_rec["cmd_pwm_log"] - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] + #log_path_record = ("$(homedir())/open_loop/output-record-8bbb.jld","$(homedir())/open_loop/output-record-3593.jld") + log_path_record = ("$(homedir())/open_loop/output-record-1486.jld",) + res_cmd = Float64[] + res_delta = Float64[] + res_t = Float64[] + L_b = 0.125 + for j=1:1 + d_rec = load(log_path_record[j]) + + imu_meas = d_rec["imu_meas"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1])+0.5 + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end])-0.5 + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] < 90 || cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] > 95 + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + end + + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + res_cmd = cat(1,res_cmd,cmd) + res_delta = cat(1,res_delta,atan2(psiDot*0.25,v_x)) + res_t = cat(1,t) + end + res_delta = res_delta[res_cmd.>0] + res_cmd = res_cmd[res_cmd.>0] + res_delta = res_delta[res_cmd.<120] + res_cmd = res_cmd[res_cmd.<120] + res_delta = res_delta[res_cmd.>60] + res_cmd = res_cmd[res_cmd.>60] + sz = size(res_cmd,1) + + cmd_left = res_cmd[res_cmd.>103] + delta_left = res_delta[res_cmd.>103] + cmd_right = res_cmd[res_cmd.<80] + delta_right = res_delta[res_cmd.<80] + + coeff = [res_cmd ones(sz,1)]\res_delta + x = [1,200] + y = [1 1;200 1]*coeff + + coeff_r = [cmd_right ones(size(cmd_right,1),1)]\delta_right + xr = [1,200] + yr = [1 1;200 1]*coeff_r + + coeff_l = [cmd_left ones(size(cmd_left,1),1)]\delta_left + xl = [1,200] + yl = [1 1;200 1]*coeff_l + + ax1=subplot(211) + plot(res_t,res_delta) + grid("on") + subplot(212,sharex=ax1) + plot(res_t,res_cmd) + grid("on") + + figure(2) + plot(res_cmd,res_delta,"*",xl,yl,xr,yr) + + plot(vel_est.t,vel_est.z) + grid("on") + legend(["vel_avg","v_fl","v_fr","v_bl","v_br"]) +end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_u_a_drag.jl b/eval_sim/Parameter estimation/find_u_a_drag.jl index c11574b8..8270e0d1 100644 --- a/eval_sim/Parameter estimation/find_u_a_drag.jl +++ b/eval_sim/Parameter estimation/find_u_a_drag.jl @@ -20,14 +20,30 @@ function main(code::AbstractString) t = t0+0.1:.02:t_end-0.1 v = zeros(length(t)) cmd = zeros(length(t)) + cmd_raw = zeros(length(t)) for i=1:length(t) - if cmd_log.z[t[i].>cmd_log.t,1][end] > 0 && vel_est.z[t[i].>vel_est.t][end] > 0.2 - v[i] = vel_est.z[t[i].>vel_est.t][end] + if cmd_log.z[t[i].>cmd_log.t,1][end] > 0 && vel_est.z[t[i].>vel_est.t,1][end] > 0.2 + v[i] = vel_est.z[t[i].>vel_est.t,1][end] cmd[i] = cmd_log.z[t[i].>cmd_log.t,1][end] + cmd_raw[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] end end #plot(cmd,v) - plot(cmd./v,"*") + figure(1) + ax1=subplot(211) + plot(vel_est.t,vel_est.z) + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br"]) + subplot(212,sharex=ax1) + plot(cmd_pwm_log.t,cmd_pwm_log.z[:,1]) + grid("on") + + figure(2) + plot(cmd,cmd./v,"*") + grid("on") + + figure(3) + plot(cmd_raw,v,"*") grid("on") end \ No newline at end of file diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index f6921a24..e2e96863 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -18,7 +18,7 @@ - + diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 953d2514..8f76c96f 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -13,7 +13,7 @@ - + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index b44ae570..f1b1133d 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -17,7 +17,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index dfb56196..ae6becc4 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -125,6 +125,8 @@ function main() posInfo.s = 0 posInfo.s_start = 0 + uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) + # Start node while ! is_shutdown() if z_est[6] > 0 # check if data has been received (s > 0) @@ -200,7 +202,7 @@ function main() tic() if lapStatus.currentLap <= 2 z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states - solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,last_u') + solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,last_u',uPrev) else # otherwise: use system-ID-model solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') end @@ -210,6 +212,8 @@ function main() uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] zCurr[i,6] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) + uPrev = circshift(uPrev,1) + uPrev[1,:] = uCurr[i,:] println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") # append new states and inputs to old trajectory diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 37510b5f..a503c99c 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -61,7 +61,7 @@ function main() buffersize = 60000 gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) - cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_log = Measurements{Float64}(1,ones(buffersize)*Inf,ones(buffersize)*Inf,zeros(buffersize,2)) z_real = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,8)) slip_a = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) @@ -119,10 +119,13 @@ function main() gps_header = Header() while ! is_shutdown() - # update current state with a new row vector - z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:],u_current', dt, modelParams) t_ros = get_rostime() t = to_sec(t_ros) + if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 + u_current[2] = cmd_log.z[t.>cmd_log.t+0.2,2][end] # artificial steering input delay + end + # update current state with a new row vector + z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:], u_current', dt, modelParams) z_real.t_msg[i] = t z_real.t[i] = t @@ -153,10 +156,10 @@ function main() if sum(dist_traveled .>= vel_dist_update)>=1 && z_current[i,3] > 0.1 # only update if at least one of the magnets has passed the sensor dist_traveled[dist_traveled.>=vel_dist_update] = 0 vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) - vel_est.vel_fl = 0 - vel_est.vel_fr = 0 - vel_est.vel_bl = 0 - vel_est.vel_br = 0 + vel_est.vel_fl = convert(Float32,0) + vel_est.vel_fr = convert(Float32,0) + vel_est.vel_bl = convert(Float32,0) + vel_est.vel_br = convert(Float32,0) end vel_est.header.stamp = t_ros publish(pub_vel, vel_est) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 9bfa80ef..0539983a 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -29,21 +29,30 @@ class low_level_control(object): str_ang_max = 35 str_ang_min = -35 ecu_pub = 0 - FxR = 0 ecu_cmd = ECU() def pwm_converter_callback(self, msg): # translate from SI units in vehicle model # to pwm angle units (i.e. to send command signal to actuators) # convert desired steering angle to degrees, saturate based on input limits - self.servo_pwm = 91.365 + 105.6*float(msg.servo) + + # Old servo control: + # self.servo_pwm = 91.365 + 105.6*float(msg.servo) + # New servo control + if msg.servo < 0.0: + self.servo_pwm = 83.6*float(msg.servo) + 92.0 + elif msg.servo > 0.0: + self.servo_pwm = 120.0*float(msg.servo) + 97.6 + else: + self.servo_pwm = 90 + # compute motor command - self.FxR = float(msg.motor) - if self.FxR == 0: + FxR = float(msg.motor) + if FxR == 0: self.motor_pwm = 90.0 - elif self.FxR > 0: - self.motor_pwm = 94.14 + 2.7678*self.FxR + elif FxR > 0: + self.motor_pwm = 94.14 + 2.7678*FxR else: # motor break / slow down - self.motor_pwm = 93.5 + 46.73*self.FxR + self.motor_pwm = 93.5 + 46.73*FxR self.update_arduino() def neutralize(self): self.motor_pwm = 60 # slow down first diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index e4e8a005..55099e98 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -58,44 +58,46 @@ function main() vel_est_log.i = 1 pos_info_log.i = 1 + chicane_speed = 1.0 + chicane_turn = -0.3 # Start node - while t < 64.0 # run exactly x seconds + while t < 34.0 # run exactly x seconds t = to_sec(get_rostime())-t0 if t <= 3 - cmd.motor = 0 - cmd.servo = 0 - # SHIKANE: - # elseif t <= 3.5 - # cmd.motor = 2 - # cmd.servo = -0.3 + cmd.motor = 0.0 + cmd.servo = 0.0 + # CHICANE: + elseif t <= 3 + cmd.motor = chicane_speed + cmd.servo = -chicane_turn # elseif t <= 4 - # cmd.motor = 2 - # cmd.servo = 0.3 - # elseif t <= 4.5 - # cmd.motor = 2 - # cmd.servo = -0.3 + # cmd.motor = chicane_speed + # cmd.servo = chicane_turn # elseif t <= 5 - # cmd.motor = 2 - # cmd.servo = 0.3 - # elseif t <= 5.5 - # cmd.motor = 2 - # cmd.servo = -0.3 + # cmd.motor = chicane_speed + # cmd.servo = -chicane_turn # elseif t <= 6 - # cmd.motor = 2 - # cmd.servo = 0.3 - # elseif t <= 6.5 - # cmd.motor = 2 - # cmd.servo = -0.3 + # cmd.motor = chicane_speed + # cmd.servo = chicane_turn # elseif t <= 7 - # cmd.motor = 2 - # cmd.servo = 0.3 - # elseif t <= 7.5 - # cmd.motor = 2 - # cmd.servo = -0.3 + # cmd.motor = chicane_speed + # cmd.servo = -chicane_turn + # elseif t <= 8 + # cmd.motor = chicane_speed + # cmd.servo = chicane_turn + # elseif t <= 9 + # cmd.motor = chicane_speed + # cmd.servo = -chicane_turn + # elseif t <= 10 + # cmd.motor = chicane_speed + # cmd.servo = chicane_turn + # elseif t <= 11 + # cmd.motor = chicane_speed + # cmd.servo = -chicane_turn # CONTINUOUS ACCELERATION: - elseif t <= 63 - cmd.motor = 1.0#0.5+(t-3)/20 - cmd.servo = -(t-3.0)/300-0.1 + elseif t <= 33 + cmd.motor = 0.2+(t-3)/20 + cmd.servo = 0#-(t-3.0)/300-0.15 # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! # cmd.servo = -0.15 diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py index 35b9cffb..169bc072 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py @@ -43,13 +43,16 @@ class StateEst(object): psiDot_meas = 0 a_x_meas = 0 a_y_meas = 0 + imu_updated = False # Velocity vel_meas = 0 + vel_updated = False # GPS x_meas = 0 y_meas = 0 + gps_updated = False # General variables t0 = 0 # Time when the estimator was started @@ -71,6 +74,7 @@ def gps_callback(self, data): #current_t = rospy.get_rostime().to_sec() self.x_meas = data.x_m self.y_meas = data.y_m + self.gps_updated = True # imu measurement update def imu_callback(self, data): @@ -103,10 +107,12 @@ def imu_callback(self, data): self.psiDot_meas = w_z self.a_x_meas = a_x self.a_y_meas = a_y + self.imu_updated = True def vel_est_callback(self, data): - if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement - self.vel_meas = data.vel_est + self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est + self.vel_meas = data.vel_est + self.vel_updated = True # state estimation node def state_estimation(): @@ -157,13 +163,32 @@ def state_estimation(): while not rospy.is_shutdown(): # make R values dependent on current measurement (robust against outliers) sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - if sq_gps_dist > 0.5: - R[0,0] = 1+10*sq_gps_dist**2 - R[1,1] = 1+10*sq_gps_dist**2 + # if sq_gps_dist > 0.5: + # R[0,0] = 1+10*sq_gps_dist**2 + # R[1,1] = 1+10*sq_gps_dist**2 + # else: + # R[0,0] = 0.1 + # R[1,1] = 0.1 + if se.gps_updated: + R[0,0] = 1.0 + R[1,1] = 1.0 else: - R[0,0] = 0.1 - R[1,1] = 0.1 + R[0,0] = 10.0 + R[1,1] = 10.0 + if se.imu_updated: + R[3,3] = 0.1 + R[4,4] = 5.0 + else: + R[3,3] = 10.0 + R[4,4] = 50.0 + if se.vel_updated: + R[2,2] = 0.1 + else: + R[2,2] = 1.0 + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False # get measurement y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas]) @@ -179,16 +204,16 @@ def state_estimation(): # Read values (x_est, y_est, psi_est, v_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate - bta = math.atan2(L_f*tan(u[1]),L_f+L_r) + bta = math.atan2(L_f*tan(u[1]), L_f+L_r) v_y_est = L_r*psi_dot_est v_x_est = cos(bta)*v_est # Update track position l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) - #l.find_s() - l.s = 0 - l.epsi = 0 - l.s_start = 0 + l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 # and then publish position info ros_t = rospy.get_rostime() diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 83a4cc39..53e7bb5d 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -173,7 +173,7 @@ def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): #print "========================" #print "bta = %f"%bta #print "psi = %f"%z[6] - zNext = [0]*4 + zNext = [0]*6 zNext[0] = z[0] + dt*(z[3]*cos(z[2] + bta)) # x zNext[1] = z[1] + dt*(z[3]*sin(z[2] + bta)) # y zNext[2] = z[2] + dt*(z[3]/l_B*sin(bta)) # psi @@ -184,7 +184,7 @@ def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): def h_KinBkMdl_2(x, u, vhMdl, dt, est_mode): """This Measurement model uses gps, imu and encoders""" - y = [0]*7 + y = [0]*5 y[0] = x[0] # x y[1] = x[1] # y y[2] = x[2]+x[5] # psi From 0b51006bc145d932aadc114d8241b33bd75121cf Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 4 Nov 2016 11:47:37 -0700 Subject: [PATCH 082/183] Fixed Arduino code. --- .../arduino_nano328_node.ino | 19 +++++++++---------- .../Parameter estimation/find_u_a_drag.jl | 8 ++++++-- workspace/src/barc/src/barc_lib | 2 +- .../src/barc/src/controller_low_level.py | 3 ++- workspace/src/barc/src/open_loop.jl | 12 ++++++------ 5 files changed, 24 insertions(+), 20 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index 05b47a1b..4939ba0d 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -448,22 +448,21 @@ unsigned long Car::getEncoder_dTime_BR() { //(ADDE } //(ADDED BY TOMMI 7JULY2016) float Car::getVelocityEstimate() { - float t_min = 2.0 // minimum time (in s) when v is set to zero, this correlates to a minimum velocity of 0.05 m/s - vel_FL = 0.0 - vel_FR = 0.0 - vel_BL = 0.0 - vel_BR = 0.0 - if(FL_DeltaTime < t_min){ + vel_FL = 0.0; + vel_FR = 0.0; + vel_BL = 0.0; + vel_BR = 0.0; + if(FL_DeltaTime > 0){ vel_FL = 2.0*3.141593*0.036/2.0*1.0/FL_DeltaTime*1000000.0; } - if(FR_DeltaTime < t_min){ + if(FR_DeltaTime > 0){ vel_FR = 2.0*3.141593*0.036/2.0*1.0/FR_DeltaTime*1000000.0; } - if(BL_DeltaTime < t_min){ + if(BL_DeltaTime > 0){ vel_BL = 2.0*3.141593*0.036/2.0*1.0/BL_DeltaTime*1000000.0; } - if(BR_DeltaTime < t_min){ + if(BR_DeltaTime > 0){ vel_BR = 2.0*3.141593*0.036/2.0*1.0/BR_DeltaTime*1000000.0; } - return ( vel_FL + vel_FR ) / 2.0 + return ( vel_FL + vel_FR ) / 2.0; } diff --git a/eval_sim/Parameter estimation/find_u_a_drag.jl b/eval_sim/Parameter estimation/find_u_a_drag.jl index 8270e0d1..3c80cb9c 100644 --- a/eval_sim/Parameter estimation/find_u_a_drag.jl +++ b/eval_sim/Parameter estimation/find_u_a_drag.jl @@ -40,10 +40,14 @@ function main(code::AbstractString) grid("on") figure(2) - plot(cmd,cmd./v,"*") + plot(cmd[cmd.>0],cmd[cmd.>0]./v[cmd.>0],"*") + xlabel("u_a") + ylabel("u_a/v") grid("on") figure(3) - plot(cmd_raw,v,"*") + plot(cmd_raw[cmd_raw.>80],v[cmd_raw.>80],"*") grid("on") + xlabel("PWM signal") + ylabel("v [m/s]") end \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index b7f435bb..431a043b 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit b7f435bb76419be048231d94bddbf1caac587b62 +Subproject commit 431a043b660ead2997e3a8a3552e651d7d656476 diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 0539983a..b8538508 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -50,7 +50,8 @@ def pwm_converter_callback(self, msg): if FxR == 0: self.motor_pwm = 90.0 elif FxR > 0: - self.motor_pwm = 94.14 + 2.7678*FxR + #self.motor_pwm = 94.14 + 2.7678*FxR + self.motor_pwm = 90.0 + 2.8*FxR else: # motor break / slow down self.motor_pwm = 93.5 + 46.73*FxR self.update_arduino() diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 55099e98..ee536548 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -61,15 +61,15 @@ function main() chicane_speed = 1.0 chicane_turn = -0.3 # Start node - while t < 34.0 # run exactly x seconds + while t < 35.0 # run exactly x seconds t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 0.0 cmd.servo = 0.0 # CHICANE: - elseif t <= 3 - cmd.motor = chicane_speed - cmd.servo = -chicane_turn + # elseif t <= 3 + # cmd.motor = chicane_speed + # cmd.servo = -chicane_turn # elseif t <= 4 # cmd.motor = chicane_speed # cmd.servo = chicane_turn @@ -96,7 +96,7 @@ function main() # cmd.servo = -chicane_turn # CONTINUOUS ACCELERATION: elseif t <= 33 - cmd.motor = 0.2+(t-3)/20 + cmd.motor = 0.0+(t-3)/10 cmd.servo = 0#-(t-3.0)/300-0.15 # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! @@ -126,7 +126,7 @@ function main() # cmd.motor = -2.0 # cmd.servo = 0 else - cmd.motor = -2.0 + cmd.motor = -0.5 cmd.servo = 0 end cmd.header.stamp = get_rostime() From 659b0596bf820677abdc3e83cb5bc537766259d8 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 9 Nov 2016 11:42:32 -0800 Subject: [PATCH 083/183] Added simple track, added 2-step-delay to Kalman filter, adapted eval scripts. --- .../Kalman simulation/sim_kalman_mix_2.jl | 51 +++-- .../Parameter estimation/find_d_f_delay.jl | 3 +- .../Parameter estimation/find_d_f_mapping.jl | 48 +++++ .../Parameter estimation/find_u_a_drag.jl | 18 +- eval_sim/create_track.py | 41 ++-- eval_sim/eval_data.jl | 201 +++++++++++------- .../src/barc/launch/open_loop_remote.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 48 +++-- .../src/barc/src/Localization_helpers.py | 23 +- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_record.jl | 4 + workspace/src/barc/src/barc_simulator_dyn.jl | 2 +- .../src/barc/src/controller_low_level.py | 17 +- workspace/src/barc/src/open_loop.jl | 48 +++-- .../src/state_estimation_KinBkMdl_mixed.py | 47 ++-- 15 files changed, 366 insertions(+), 189 deletions(-) diff --git a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl index 9480d265..180f0a52 100644 --- a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl +++ b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl @@ -27,7 +27,8 @@ R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) function main(code::AbstractString) global Q, R, R_gps_imu - log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) imu_meas = d_rec["imu_meas"] @@ -37,8 +38,8 @@ function main(code::AbstractString) vel_est = d_rec["vel_est"] pos_info = d_rec["pos_info"] - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1])+0.3 - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end])-0.3 + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_pwm_log.t[1])+0.0 + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end])-0.0 l_A = 0.125 l_B = 0.125 @@ -62,8 +63,8 @@ function main(code::AbstractString) yaw_prev = yaw0 - Q_gps_imu = diagm([1.0,1.0,1.0,0.1,0.1,0.01]) - R_gps_imu = diagm([1.0,1.0,0.1,0.1,5.0]) + Q_gps_imu = diagm([0.1,0.1,1.0,0.1,0.1,0.01]) + R_gps_imu = diagm([1.0,1.0,10000.0,0.1,5.0]) gps_gate = zeros(length(t)) @@ -86,7 +87,7 @@ function main(code::AbstractString) y_gps_imu[:,3] = unwrap!(y_gps_imu[:,3]) u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] - u[i,2] = cmd_log.z[t[i].>cmd_log.t-0.2,2][end] + u[i,2] = cmd_log.z[t[i].>cmd_log.t-0.0,2][end] gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) #gps_gate[i] = @@ -150,26 +151,36 @@ function main(code::AbstractString) title("Velocity estimate and measurement") figure(4) - plot(t-t0,x_est_gps_imu[:,3],"-*",t-t0,y_gps_imu[:,3]) + plot(t-t0,y_gps_imu[:,3],t-t0,x_est_gps_imu[:,3],"-*",pos_info.t-t0,pos_info.z[:,10]) grid("on") title("Comparison yaw") - legend(["psi_est","psi_meas"]) + legend(["psi_meas","psi_est","psi_onboard"]) - figure(5) - plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,5]) - title("w_z") - legend(["w_z_meas","w_z_est"]) - grid("on") + # figure(5) + # plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,5]) + # title("w_z") + # legend(["w_z_meas","w_z_est"]) + # grid("on") - figure(6) - plot(t-t0,x_est_gps_imu[:,7:8]) - grid("on") - legend(["v_x_est","v_y_est"]) + # figure(6) + # plot(t-t0,x_est_gps_imu[:,7:8]) + # grid("on") + # legend(["v_x_est","v_y_est"]) - figure(7) - plot(t-t0,gps_gate,t-t0,gps_dist,gps_meas.t-t0,gps_meas.z) + # figure(7) + # plot(t-t0,gps_gate,t-t0,gps_dist,gps_meas.t-t0,gps_meas.z) + # grid("on") + # legend(["gate","dist"]) + + figure(8) + plot(x_est_gps_imu[:,1],x_est_gps_imu[:,2]) + for i=1:5:size(x_est_gps_imu,1) + ar = [x_est_gps_imu[i,1] x_est_gps_imu[i,2]; + x_est_gps_imu[i,1]+0.2*cos(x_est_gps_imu[i,3]) x_est_gps_imu[i,2]+0.2*sin(x_est_gps_imu[i,3])] + plot(ar[:,1],ar[:,2]) + end + axis("equal") grid("on") - legend(["gate","dist"]) # # CORRELATIONS: # figure(8) diff --git a/eval_sim/Parameter estimation/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl index e3cf1025..4ff3d49a 100644 --- a/eval_sim/Parameter estimation/find_d_f_delay.jl +++ b/eval_sim/Parameter estimation/find_d_f_delay.jl @@ -3,7 +3,8 @@ using PyPlot using JLD function main(code::AbstractString) - log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) L_b = 0.125 diff --git a/eval_sim/Parameter estimation/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl index bc22d70a..f56cf349 100644 --- a/eval_sim/Parameter estimation/find_d_f_mapping.jl +++ b/eval_sim/Parameter estimation/find_d_f_mapping.jl @@ -82,4 +82,52 @@ function main() plot(vel_est.t,vel_est.z) grid("on") legend(["vel_avg","v_fl","v_fr","v_bl","v_br"]) +end + +function main_pwm(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + + L_b = 0.125 + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1])+1.0 + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end])-1.0 + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] < 90 || cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] > 95 + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] == cmd_pwm_log.z[t[i]-1.0.>cmd_pwm_log.t,2][end] + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + end + end + + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + delta = atan2(psiDot*0.25,v_x) + + idx = copy( (delta.<1.0) & (delta.> -1.0) & (cmd .< 90) & (cmd .> 50) ) + delta = delta[idx] + cmd = cmd[idx] + + sz = size(cmd,1) + coeff = [cmd ones(sz,1)]\delta + x = [1,200] + y = [1 1;200 1]*coeff + + plot(cmd,delta,"*",x,y) + grid("on") + + println("c1 = $(1/coeff[1])") + println("c2 = $(coeff[2]/coeff[1])") + end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_u_a_drag.jl b/eval_sim/Parameter estimation/find_u_a_drag.jl index 3c80cb9c..20c20862 100644 --- a/eval_sim/Parameter estimation/find_u_a_drag.jl +++ b/eval_sim/Parameter estimation/find_u_a_drag.jl @@ -29,6 +29,17 @@ function main(code::AbstractString) cmd_raw[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] end end + + idx = (v.>1) & (cmd_raw .> 95) + + sz = size(v[idx],1) + A = [cmd_raw[idx] ones(sz,1)] + b = v[idx] + coeff = A\b + + x = [90,110] + y = [90 1;110 1]*coeff + #plot(cmd,v) figure(1) ax1=subplot(211) @@ -46,8 +57,11 @@ function main(code::AbstractString) grid("on") figure(3) - plot(cmd_raw[cmd_raw.>80],v[cmd_raw.>80],"*") + plot(cmd_raw[cmd_raw.>80],v[cmd_raw.>80],"*",x,y) grid("on") xlabel("PWM signal") ylabel("v [m/s]") -end \ No newline at end of file + + println("c1 = $(1/coeff[1])") + println("c2 = $(coeff[2]/coeff[1])") +end diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index 65671c41..9baff604 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -23,21 +23,36 @@ def add_curve(theta, length, angle): theta = array([0]) +# SOPHISTICATED TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,60,-2*pi/3) +# theta = add_curve(theta,90,pi) +# theta = add_curve(theta,80,-5*pi/6) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,50,0) +# theta = add_curve(theta,40,-pi/4) +# theta = add_curve(theta,30,pi/4) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,25,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,28,0) + +# GOGGLE TRACK theta = add_curve(theta,30,0) -theta = add_curve(theta,60,-2*pi/3) -theta = add_curve(theta,90,pi) -theta = add_curve(theta,80,-5*pi/6) -theta = add_curve(theta,10,0) -theta = add_curve(theta,50,-pi/2) -theta = add_curve(theta,50,0) -theta = add_curve(theta,40,-pi/4) -theta = add_curve(theta,30,pi/4) -theta = add_curve(theta,20,0) -theta = add_curve(theta,50,-pi/2) -theta = add_curve(theta,25,0) -theta = add_curve(theta,50,-pi/2) -theta = add_curve(theta,28,0) +theta = add_curve(theta,40,-pi/2) +#theta = add_curve(theta,10,0) +theta = add_curve(theta,40,-pi/2) +theta = add_curve(theta,20,-pi/6) +theta = add_curve(theta,30,pi/3) +theta = add_curve(theta,20,-pi/6) +theta = add_curve(theta,40,-pi/2) +#theta = add_curve(theta,10,0) +theta = add_curve(theta,40,-pi/2) +theta = add_curve(theta,35,0) +# SIMPLE TRACK # theta = add_curve(theta,50,0) # theta = add_curve(theta,100,-pi) # theta = add_curve(theta,100,0) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index ebbe4e18..9d91e2bd 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -116,13 +116,12 @@ function eval_run(code::AbstractString) axis("equal") legend(["GPS meas","estimate"]) - figure() - plot(imu_meas.t-t0,imu_meas.z[:,7:9]) - grid("on") - title("Measured accelerations") + # figure() + # plot(imu_meas.t-t0,imu_meas.z[:,7:9]) + # grid("on") + # title("Measured accelerations") figure() - title("x/y measurements and estimate") plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x") grid(1) title("GPS comparison") @@ -132,6 +131,7 @@ function eval_run(code::AbstractString) figure() ax2=subplot(211) + title("Commands") plot(cmd_log.t,cmd_log.z,"-*",cmd_log.t_msg,cmd_log.z,"-x") grid("on") xlabel("t [s]") @@ -146,34 +146,28 @@ function eval_run(code::AbstractString) legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift"]) grid() - figure() - title("Raw IMU orientation data") - plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6]) - grid("on") - legend(["w_x","w_y","w_z","roll","pitch","yaw"]) + # figure() + # title("Raw IMU orientation data") + # plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6]) + # grid("on") + # legend(["w_x","w_y","w_z","roll","pitch","yaw"]) figure() title("v measurements and estimate") - plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z,"-x",vel_est.t_msg-t0,vel_est.z,"-+") + plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z,"-x") legend(["est_xDot","est_yDot","v_raw"]) grid() - figure() - title("Acceleration data") - plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") - legend(["a_x","a_y","a_z"]) - grid() - - figure() - title("s, eY and inputs") - ax1=subplot(211) - plot(pos_info.t-t0,pos_info.z[:,2:3],"-*") - grid("on") - legend(["eY","ePsi"]) - subplot(212,sharex=ax1) - plot(cmd_log.t-t0,cmd_log.z,"-*") - grid("on") - legend(["u","d_f"]) + # figure() + # title("s, eY and inputs") + # ax1=subplot(211) + # plot(pos_info.t-t0,pos_info.z[:,2:3],"-*") + # grid("on") + # legend(["eY","ePsi"]) + # subplot(212,sharex=ax1) + # plot(cmd_log.t-t0,cmd_log.z,"-*") + # grid("on") + # legend(["u","d_f"]) nothing end @@ -261,13 +255,13 @@ function eval_LMPC(code::AbstractString) t0 = t[1] - figure() + figure(1) plot(t-t0,step_diff) grid("on") title("One-step-errors") legend(["xDot","yDot","psiDot","ePsi","eY"]) - figure() + figure(2) ax1=subplot(311) plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*") title("Comparison of publishing and receiving time") @@ -285,32 +279,71 @@ function eval_LMPC(code::AbstractString) grid("on") legend(["1","2","3"]) - figure() - c = zeros(size(curv,1),1) - for i=1:size(curv,1) - s = state[i,1] - c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + figure(3) + plot(t,state) + grid("on") + legend(["v_x","v_y","psiDot","ePsi","eY","s"]) + + figure(4) + title("Open loop predictions") + #plot(t,state[:,[1,4,5]]) + for i=1:4:size(t,1)-10 + if sol_z[1,5,i]==0 + plot(t[i:i+10]-t0,sol_z[:,2:4,i]) + else + plot(t[i:i+10]-t0,sol_z[:,1:5,i]) + end end - plot(s_start+state[:,1],c,"-o") - for i=1:2:size(curv,1) - s = sol_z[:,1,i] - c = zeros(size(curv,1),1) - c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - plot(s_start[i]+s,c,"-*") + grid("on") + + figure() + title("Open loop inputs") + for i=1:4:size(t,1)-10 + plot(t[i:i+7]-t0,sol_u[1:8,2,i],t[i:i+9]-t0,sol_u[:,1,i]) end - title("Curvature over path") - xlabel("Curvilinear abscissa [m]") - ylabel("Curvature") - grid() + grid("on") - track = create_track(0.3) figure() - hold(1) - plot(x_est[:,1],x_est[:,2],"-*") - title(["Estimated position"]) - plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - axis("equal") - grid(1) + subplot(311) + title("c_Vx") + plot(t-t0,c_Vx) + grid("on") + subplot(312) + title("c_Vy") + plot(t-t0,c_Vy) + grid("on") + subplot(313) + title("c_Psi") + plot(t-t0,c_Psi) + grid("on") + + # *********** CURVATURE ********************* + # figure() + # c = zeros(size(curv,1),1) + # for i=1:size(curv,1) + # s = state[i,6]-s_start[i] + # c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + # end + # plot(s_start+state[:,1],c,"-o") + # for i=1:2:size(curv,1) + # s = sol_z[:,1,i] + # c = zeros(size(curv,1),1) + # c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + # plot(s_start[i]+s,c,"-*") + # end + # title("Curvature over path") + # xlabel("Curvilinear abscissa [m]") + # ylabel("Curvature") + # grid() + + # track = create_track(0.3) + # figure() + # hold(1) + # plot(x_est[:,1],x_est[:,2],"-*") + # title("Estimated position") + # plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + # axis("equal") + # grid(1) # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA: # CURRENT HEADING (PLOTTED BY A LINE) # for i=1:size(x_est,1) @@ -322,14 +355,14 @@ function eval_LMPC(code::AbstractString) # end # PREDICTED PATH - for i=1:4:size(x_est,1) - z_pred = zeros(11,4) - z_pred[1,:] = x_est[i,:] - for j=2:11 - z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) - end - plot(z_pred[:,1],z_pred[:,2],"-*") - end + # for i=1:4:size(x_est,1) + # z_pred = zeros(11,4) + # z_pred[1,:] = x_est[i,:] + # for j=2:11 + # z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + # end + # plot(z_pred[:,1],z_pred[:,2],"-*") + # end # PREDICTED REFERENCE PATH (DEFINED BY POLYNOM) # for i=1:size(x_est,1) @@ -351,17 +384,15 @@ function eval_LMPC(code::AbstractString) # end figure() - plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-o") + plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-x") + title("Old Trajectory") legend(["v_x","v_y","psiDot","ePsi","eY"]) + xlabel("s") grid(1) figure() - plot(s_start+state[:,1],state[:,[2,4]],"*") - grid() - - figure() - title("MPC states and cost") ax1=subplot(211) + title("MPC states and cost") plot(t-t0,state) legend(["v_x","v_y","psiDot","ePsi","eY","s"]) grid(1) @@ -392,7 +423,8 @@ function eval_oldTraj(code::AbstractString,i::Int64) legend(["v_x","v_x","psiDot","ePsi","eY","s"]) end -function eval_LMPC_coeff(k) +function eval_LMPC_coeff(code::AbstractString,k::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" d = load(log_path_LMPC) oldTraj = d["oldTraj"] sol_z = d["sol_z"] @@ -512,13 +544,11 @@ function anim_curv(curv) end end - - -function eval_prof() - Profile.clear() - @load "$(homedir())/simulations/profile.jlprof" - ProfileView.view(li, lidict=lidict) -end +# function eval_prof() +# Profile.clear() +# @load "$(homedir())/simulations/profile.jlprof" +# ProfileView.view(li, lidict=lidict) +# end function create_track(w) x = [0.0] # starting point @@ -554,12 +584,24 @@ function create_track(w) # add_curve(theta,100,-pi) # add_curve(theta,49,0) - # SHORT SIMPLE track - add_curve(theta,10,0) - add_curve(theta,80,-pi) - add_curve(theta,20,0) - add_curve(theta,80,-pi) - add_curve(theta,9,0) + # GOGGLE TRACK + add_curve(theta,30,0) + add_curve(theta,40,-pi/2) + #add_curve(theta,10,0) + add_curve(theta,40,-pi/2) + add_curve(theta,20,-pi/6) + add_curve(theta,30,pi/3) + add_curve(theta,20,-pi/6) + add_curve(theta,40,-pi/2) + #add_curve(theta,10,0) + add_curve(theta,40,-pi/2) + add_curve(theta,35,0) + # # SHORT SIMPLE track + # add_curve(theta,10,0) + # add_curve(theta,80,-pi) + # add_curve(theta,20,0) + # add_curve(theta,80,-pi) + # add_curve(theta,9,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) @@ -570,6 +612,7 @@ function create_track(w) push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) end track = cat(2, x, y, x_l, y_l, x_r, y_r) + #plot(track[:,1],track[:,2]) return track #plot(x,y,x_l,y_l,x_r,y_r) end diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 8f76c96f..4abac3ea 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -29,7 +29,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index ae6becc4..eb4de379 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -106,7 +106,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 12.0#24.0 + posInfo.s_target = 17.76# 12.0#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -127,30 +127,30 @@ function main() uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) + n_pf = 2 # number of first path-following laps (needs to be at least 2) + # Start node while ! is_shutdown() if z_est[6] > 0 # check if data has been received (s > 0) - # ============================= PUBLISH COMMANDS ============================= - # this is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received - # the state is predicted by 0.1s - println("Publishing command...") + # This is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received + # This guarantees a constant publishing frequency of 10 Hz + # (The state can be predicted by 0.1s) cmd.header.stamp = get_rostime() cmd.motor = convert(Float32,mpcSol.a_x) cmd.servo = convert(Float32,mpcSol.d_f) - publish(pub, cmd) - println("Finished publishing.") + publish(pub, cmd) # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr[i,:] = copy(z_est) # update state information (actually predicted by Kalman filter!) + zCurr[i,:] = copy(z_est) # update state information posInfo.s = zCurr[i,6] # update position info posInfo.s_start = copy(s_start_update[1]) # use a copy so s_start is not updated during one iteration trackCoeff.coeffCurvature = copy(coeffCurvature_update) # ============================= Pre-Logging (before solving) ================================ - log_t[k+1] = time() # time is measured *before* solving (more consistent that way) - if lapStatus.currentLap <=2 # find 1-step-error - step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[zCurr[i,1], 0, 0, zCurr[i,4], zCurr[i,5]]).^2 + log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) + if lapStatus.currentLap <= n_pf # find 1-step-error + step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]).^2 else step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 end @@ -166,7 +166,7 @@ function main() println("oldTraj: $(oldTraj.oldTraj[:,:,1])") log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] zCurr[1,:] = zCurr[i,:] # copy current state - u_final  = uCurr[i-1,:] # ... and input + u_final  = uCurr[i-1,:] # ... and input i = 1 lapStatus.currentIt = 1 # reset current iteration lapStatus.currentLap += 1 # start next lap @@ -184,7 +184,7 @@ function main() println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") # Find coefficients for cost and constraints - if lapStatus.currentLap > 2 + if lapStatus.currentLap > n_pf tic() coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) tt = toq() @@ -200,16 +200,17 @@ function main() println("Starting solving.") # Solve the MPC problem tic() - if lapStatus.currentLap <= 2 + if lapStatus.currentLap <= n_pf z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states - solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,last_u',uPrev) + solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) else # otherwise: use system-ID-model + #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') end tt = toq() # Write current input information - uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] + uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] zCurr[i,6] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) uPrev = circshift(uPrev,1) @@ -247,7 +248,7 @@ function main() log_c_Vx[k,:] = mpcCoeff.c_Vx log_c_Vy[k,:] = mpcCoeff.c_Vy log_c_Psi[k,:] = mpcCoeff.c_Psi - if lapStatus.currentLap <= 2 + if lapStatus.currentLap <= n_pf log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) else log_sol_z[:,1:6,k] = mpcSol.z @@ -263,6 +264,10 @@ function main() # Save simulation data to file log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4]).jld" + if isfile(log_path) + log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4])-2.jld" + warn("Warning: File already exists.") + end save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], @@ -287,4 +292,11 @@ end # Definitions of variables: # zCurr contains all state information from the beginning of the lap (first s >= 0) to the current state i # uCurr -> same as zCurr for inputs -# generally: zCurr[i+1] = f(zCurr[i],uCurr[i]) \ No newline at end of file +# generally: zCurr[i+1] = f(zCurr[i],uCurr[i]) + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s \ No newline at end of file diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 458d0196..a42a97d6 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -142,12 +142,25 @@ def create_track(self): # theta = add_curve(theta,100,-pi) # theta = add_curve(theta,49,0) + # GOGGLE TRACK: length = 17.76m + theta = add_curve(theta,30,0) + theta = add_curve(theta,40,-pi/2) + #theta = add_curve(theta,10,0) + theta = add_curve(theta,40,-pi/2) + theta = add_curve(theta,20,-pi/6) + theta = add_curve(theta,30,pi/3) + theta = add_curve(theta,20,-pi/6) + theta = add_curve(theta,40,-pi/2) + #theta = add_curve(theta,10,0) + theta = add_curve(theta,40,-pi/2) + theta = add_curve(theta,35,0) + # SHORT SIMPLE RACETRACK (smooth curves): length = 24.0m - theta = add_curve(theta,10,0) - theta = add_curve(theta,80,-pi) - theta = add_curve(theta,20,0) - theta = add_curve(theta,80,-pi) - theta = add_curve(theta,9,0) + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,80,-pi) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi) + # theta = add_curve(theta,9,0) # SIMPLER RACETRACK (half circles as curves): diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 431a043b..dc46580a 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 431a043b660ead2997e3a8a3552e651d7d656476 +Subproject commit dc46580a385d2c2fa7ba6a584ce06a95825d5d73 diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 7af3cc0f..bf474556 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -61,6 +61,10 @@ function main() # Save simulation data to file #log_path = "$(homedir())/simulations/record-$(Dates.format(now(),"yyyy-mm-dd-HH-MM-SS")).jld" log_path = "$(homedir())/simulations/output-record-$(run_id[1:4]).jld" + if isfile(log_path) + log_path = "$(homedir())/simulations/output-record-$(run_id[1:4])-2.jld" + warn("Warning: File already exists.") + end save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) println("Exiting node... Saving recorded data to $log_path.") end diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index a503c99c..6198fe8a 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -122,7 +122,7 @@ function main() t_ros = get_rostime() t = to_sec(t_ros) if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 - u_current[2] = cmd_log.z[t.>cmd_log.t+0.2,2][end] # artificial steering input delay + u_current[2] = cmd_log.z[t.>cmd_log.t+0.0,2][end] # artificial steering input delay end # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:], u_current', dt, modelParams) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index b8538508..5141c84c 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -34,14 +34,14 @@ def pwm_converter_callback(self, msg): # translate from SI units in vehicle model # to pwm angle units (i.e. to send command signal to actuators) # convert desired steering angle to degrees, saturate based on input limits - + # Old servo control: # self.servo_pwm = 91.365 + 105.6*float(msg.servo) - # New servo control - if msg.servo < 0.0: - self.servo_pwm = 83.6*float(msg.servo) + 92.0 - elif msg.servo > 0.0: - self.servo_pwm = 120.0*float(msg.servo) + 97.6 + # New servo Control + if msg.servo < 0.0: # right curve + self.servo_pwm = 95.5 + 118.8*float(msg.servo) + elif msg.servo > 0.0: # left curve + self.servo_pwm = 90.8 + 78.9*float(msg.servo) else: self.servo_pwm = 90 @@ -50,8 +50,9 @@ def pwm_converter_callback(self, msg): if FxR == 0: self.motor_pwm = 90.0 elif FxR > 0: - #self.motor_pwm = 94.14 + 2.7678*FxR - self.motor_pwm = 90.0 + 2.8*FxR + #self.motor_pwm = 94.14 + 2.7678*FxR # using write() in Arduino + self.motor_pwm = 87.6 + 4.83*FxR # using writeMicroseconds() in Arduino + #self.motor_pwm = 90.0 + 2.8*FxR else: # motor break / slow down self.motor_pwm = 93.5 + 46.73*FxR self.update_arduino() diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index ee536548..590cf72d 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -39,7 +39,7 @@ function main() run_id = get_param("run_id") loop_rate = Rate(10) - pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + pub = Publisher("ecu_pwm", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} cmd = ECU() # command type cmd.motor = 0 cmd.servo = 0 @@ -61,7 +61,7 @@ function main() chicane_speed = 1.0 chicane_turn = -0.3 # Start node - while t < 35.0 # run exactly x seconds + while t < 29.0 # run exactly x seconds t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 0.0 @@ -95,24 +95,24 @@ function main() # cmd.motor = chicane_speed # cmd.servo = -chicane_turn # CONTINUOUS ACCELERATION: - elseif t <= 33 - cmd.motor = 0.0+(t-3)/10 - cmd.servo = 0#-(t-3.0)/300-0.15 - # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.15 - # elseif t <= 13 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.2 - # elseif t <= 18 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.25 - # elseif t <= 23 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.3 - # elseif t <= 28 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.35 + # elseif t <= 123 + # cmd.motor = 0.0+(t-3)/20 + # cmd.servo = 0.0#-(t-3.0)/300-0.15 + elseif t <= 8 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 100.0 + elseif t <= 13 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 105.0 + elseif t <= 18 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 110.0 + elseif t <= 23 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 115.0 + elseif t <= 28 # CHECK TIME AND ACCELERATION !!! + cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + cmd.servo = 120.0 # elseif t <= 33 # CHECK TIME AND ACCELERATION !!! # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! # cmd.servo = -0.4 @@ -126,8 +126,8 @@ function main() # cmd.motor = -2.0 # cmd.servo = 0 else - cmd.motor = -0.5 - cmd.servo = 0 + cmd.motor = -1.0 + cmd.servo = 90.0 end cmd.header.stamp = get_rostime() publish(pub, cmd) @@ -145,6 +145,10 @@ function main() println("Exiting open loop control.") log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4]).jld" + if isfile(log_path) + log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4])-2.jld" + warn("Warning: File already exists.") + end save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) println("Exiting node... Saved recorded data to $log_path.") end diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py index 169bc072..d658d4b9 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py @@ -19,7 +19,7 @@ from sensor_msgs.msg import Imu from marvelmind_nav.msg import hedge_pos from std_msgs.msg import Header -from numpy import eye, array, zeros, diag, unwrap, tan, cos +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin from observers import ekf from system_models import f_KinBkMdl_2, h_KinBkMdl_2 from tf import transformations @@ -48,6 +48,8 @@ class StateEst(object): # Velocity vel_meas = 0 vel_updated = False + vel_prev = 0 + vel_count = 0 # this counts how often the same vel measurement has been received # GPS x_meas = 0 @@ -110,9 +112,17 @@ def imu_callback(self, data): self.imu_updated = True def vel_est_callback(self, data): - self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est - self.vel_meas = data.vel_est - self.vel_updated = True + #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est + if data.vel_est != self.vel_prev: + self.vel_meas = data.vel_est + self.vel_updated = True + self.vel_prev = data.vel_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True # state estimation node def state_estimation(): @@ -147,7 +157,7 @@ def state_estimation(): z_EKF = zeros(6) # x, y, psi, v, psi_drift P = eye(6) # initial dynamics coveriance matrix - Q = diag([1.0,1.0,1.0,0.1,0.1,0.01]) + Q = diag([0.1,0.1,1.0,0.1,0.1,0.01]) R = diag([1.0,1.0,0.1,0.1,5.0]) # Set up track parameters @@ -159,24 +169,24 @@ def state_estimation(): # Estimation variables (x_est, y_est) = [0]*2 + bta = 0 + v_est = 0 + psi_est = 0 while not rospy.is_shutdown(): # make R values dependent on current measurement (robust against outliers) sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - # if sq_gps_dist > 0.5: - # R[0,0] = 1+10*sq_gps_dist**2 - # R[1,1] = 1+10*sq_gps_dist**2 - # else: - # R[0,0] = 0.1 - # R[1,1] = 0.1 - if se.gps_updated: - R[0,0] = 1.0 - R[1,1] = 1.0 - else: + if se.gps_updated and sq_gps_dist < 0.5: # if there's a new gps value: R[0,0] = 10.0 R[1,1] = 10.0 + else: + # otherwise just extrapolate measurements: + se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) + se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) + R[0,0] = 100.0 + R[1,1] = 100.0 if se.imu_updated: - R[3,3] = 0.1 + R[3,3] = 1.0 R[4,4] = 5.0 else: R[3,3] = 10.0 @@ -193,7 +203,7 @@ def state_estimation(): y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas]) # define input - d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of the steering + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering u = [se.cmd_motor, d_f_hist.pop(0)] # build extra arguments for non-linear function @@ -205,7 +215,8 @@ def state_estimation(): # Read values (x_est, y_est, psi_est, v_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate bta = math.atan2(L_f*tan(u[1]), L_f+L_r) - v_y_est = L_r*psi_dot_est + v_y_est = L_r*psi_dot_est # problem: results in linear dependency in system ID + v_y_est = sin(bta)*v_est v_x_est = cos(bta)*v_est # Update track position From 52a9adbd5c59a3b766189d63f671b0751bfc9539 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 9 Nov 2016 14:07:54 -0800 Subject: [PATCH 084/183] Started working on more general oldTraj structure. --- workspace/src/barc/src/LMPC_node.jl | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index eb4de379..794e4f03 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -19,7 +19,7 @@ include("barc_lib/LMPC/coeffConstraintCost.jl") include("barc_lib/LMPC/solveMpcProblem.jl") include("barc_lib/LMPC/functions.jl") -function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, +function SE_callback(msg::pos_info,lapStatus::LapStatus,oldTraj::OldTrajectory,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data # update mpc initial condition z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer @@ -28,6 +28,10 @@ function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature x_est[:] = [msg.x,msg.y,msg.psi,msg.v] coeffX[:] = msg.coeffX coeffY[:] = msg.coeffY + + # save current state in oldTraj + oldTraj[oldTraj.count,:,lapStatus.currentLap] = z_est + oldTraj.count += 1 end function main() @@ -89,7 +93,7 @@ function main() loop_rate = Rate(10) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,oldTraj,s_start_update,lapStatus,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} run_id = get_param("run_id") println("Finished initialization.") From 28d5d697d7002f8f5c4686fa23308d523ca5448d Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 9 Nov 2016 14:47:29 -0800 Subject: [PATCH 085/183] Removed s_start --- workspace/src/barc/src/LMPC_node.jl | 26 +++++++------------ .../src/barc/src/Localization_helpers.py | 16 +++++++----- workspace/src/barc/src/barc_lib | 2 +- 3 files changed, 19 insertions(+), 25 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index eb4de379..bedf57f4 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -19,11 +19,10 @@ include("barc_lib/LMPC/coeffConstraintCost.jl") include("barc_lib/LMPC/solveMpcProblem.jl") include("barc_lib/LMPC/functions.jl") -function SE_callback(msg::pos_info,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, +function SE_callback(msg::pos_info,coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data # update mpc initial condition z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer - s_start_update[1] = msg.s_start coeffCurvature_update[:] = msg.coeffCurvature x_est[:] = [msg.x,msg.y,msg.psi,msg.v] coeffX[:] = msg.coeffX @@ -60,7 +59,6 @@ function main() x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) coeffX = zeros(9) # buffer for coeffX (only logging) coeffY = zeros(9) # buffer for coeffY (only logging) - s_start_update = [0.0] # buffer for s_start cmd = ECU() # command type coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) @@ -70,7 +68,6 @@ function main() log_sol_z = zeros(mpcParams.N+1,6,10000) log_sol_u = zeros(mpcParams.N,2,10000) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) - log_s_start = zeros(10000) log_state_x = zeros(10000,4) log_coeffX = zeros(10000,9) log_coeffY = zeros(10000,9) @@ -88,14 +85,14 @@ function main() init_node("mpc_traj") loop_rate = Rate(10) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} - # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (s_start_update,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: + s1 = Subscriber("pos_info", pos_info, SE_callback, (coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} run_id = get_param("run_id") println("Finished initialization.") # Lap parameters switchLap = false # initialize lap lap trigger - s_lapTrigger = 0.3 # next lap is triggered in the interval s_start in [0,s_lapTrigger] + s_lapTrigger = 0.3 # next lap is triggered in the interval s in [0,s_lapTrigger] # buffer in current lap zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) @@ -118,12 +115,10 @@ function main() oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) posInfo.s = posInfo.s_target/2 - posInfo.s_start = 0 coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) oldTraj.oldTraj[1:buffersize,6,1] = zeros(buffersize,1) oldTraj.oldTraj[1:buffersize,6,2] = zeros(buffersize,1) posInfo.s = 0 - posInfo.s_start = 0 uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) @@ -144,7 +139,6 @@ function main() i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information posInfo.s = zCurr[i,6] # update position info - posInfo.s_start = copy(s_start_update[1]) # use a copy so s_start is not updated during one iteration trackCoeff.coeffCurvature = copy(coeffCurvature_update) # ============================= Pre-Logging (before solving) ================================ @@ -157,7 +151,7 @@ function main() log_step_diff[k+1,:] = step_diff # ======================================= Lap trigger ======================================= - if (posInfo.s_start + posInfo.s)%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... + if posInfo.s%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... println("Finishing one lap at iteration $i") println("current state: $(zCurr[i,:])") println("previous state: $(zCurr[i-1,:])") @@ -171,7 +165,7 @@ function main() lapStatus.currentIt = 1 # reset current iteration lapStatus.currentLap += 1 # start next lap switchLap = false - elseif (posInfo.s_start+posInfo.s)%posInfo.s_target > s_lapTrigger + elseif posInfo.s%posInfo.s_target > s_lapTrigger switchLap = true end @@ -180,8 +174,7 @@ function main() println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") println("State Nr. $i = $z_est") println("s = $(posInfo.s)") - println("s_start = $(posInfo.s_start)") - println("s_total = $((posInfo.s+posInfo.s_start)%posInfo.s_target)") + println("s_total = $(posInfo.s%posInfo.s_target)") # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf @@ -211,7 +204,7 @@ function main() # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] - zCurr[i,6] = (posInfo.s_start + posInfo.s)%posInfo.s_target # save absolute position in s (for oldTrajectory) + zCurr[i,6] = posInfo.s%posInfo.s_target # save absolute position in s (for oldTrajectory) uPrev = circshift(uPrev,1) uPrev[1,:] = uCurr[i,:] @@ -243,7 +236,6 @@ function main() log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst log_cost[k,:] = mpcSol.cost log_curv[k,:] = trackCoeff.coeffCurvature - log_s_start[k] = posInfo.s_start log_state_x[k,:] = x_est log_c_Vx[k,:] = mpcCoeff.c_Vx log_c_Vy[k,:] = mpcCoeff.c_Vy @@ -270,7 +262,7 @@ function main() end save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, - "s_start",log_s_start[1:k],"x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], + "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) println("Exiting LMPC node. Saved data to $log_path.") diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index a42a97d6..cef4d11e 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -268,6 +268,8 @@ def find_s(self): # Use closest node to determine start and end of polynomial approximation idx_start = idx_min - self.N_nodes_poly_back idx_end = idx_min + self.N_nodes_poly_front + + s_start = idx_start * self.ds if self.closed == True: # if the track is modeled as closed (start = end) if idx_start<0: # and if the start for polynomial approx. is before the start line nodes_X = hstack((self.nodes[0,n+idx_start:n],self.nodes[0,0:idx_end+1])) # then stack the end and beginning of a lap together @@ -299,14 +301,14 @@ def find_s(self): Matrix = zeros([self.nPoints,self.OrderXY+1]) for i in range(0,self.nPoints): for k in range(0,self.OrderXY+1): - Matrix[i,self.OrderXY-k] = (i*self.ds)**k + Matrix[i,self.OrderXY-k] = (s_start + i*self.ds)**k # curvature matrix Matrix3rd = zeros([self.nPoints,self.OrderThetaCurv+1]) for i in range(0,self.nPoints): for k in range(0,self.OrderThetaCurv+1): - Matrix3rd[i,self.OrderThetaCurv-k] = (i*self.ds)**k - + Matrix3rd[i,self.OrderThetaCurv-k] = (s_start + i*self.ds)**k + # Solve system of equations to find polynomial coefficients (for x-y) self.coeffX = linalg.lstsq(Matrix,nodes_X)[0] self.coeffY = linalg.lstsq(Matrix,nodes_Y)[0] @@ -316,7 +318,7 @@ def find_s(self): b_curvature_vector = zeros(self.nPoints) for j in range(0,self.nPoints): - s = j*self.ds + s = s_start + j*self.ds dX = polyval(polyder(self.coeffX,1),s) dY = polyval(polyder(self.coeffY,1),s) ddX = polyval(polyder(self.coeffX,2),s) @@ -345,9 +347,9 @@ def find_s(self): # Calculate s - discretization = 0.0001 # discretization to calculate s + discretization = 0.001 # discretization to calculate s - j = arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) + j = s_start + arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) #print "idx_min = %f"%idx_min #print "s_idx_start = %f"%s_idx_start #print "idx_start = %f"%idx_start @@ -393,7 +395,7 @@ def find_s(self): self.s = s self.coeffTheta = coeffTheta self.coeffCurvature = coeffCurvature - self.s_start = idx_start*self.ds + self.s_start = s_start def __init__(self): diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index dc46580a..3c683d82 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit dc46580a385d2c2fa7ba6a584ce06a95825d5d73 +Subproject commit 3c683d826069bb7fdc2476705e6a26acb21e38a3 From 58aa4f978cba821f48e696f98d1b5226d7b090dc Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 10 Nov 2016 11:37:55 -0800 Subject: [PATCH 086/183] Changed oldTraj structure --- eval_sim/eval_data.jl | 67 ++-- workspace/src/barc/src/LMPC_node.jl | 105 +++--- workspace/src/barc/src/LMPC_node.jl.orig | 307 ++++++++++++++++++ workspace/src/barc/src/barc_lib | 2 +- .../src/state_estimation_KinBkMdl_mixed.py | 31 +- 5 files changed, 427 insertions(+), 85 deletions(-) create mode 100755 workspace/src/barc/src/LMPC_node.jl.orig diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 9d91e2bd..938710c1 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -132,11 +132,11 @@ function eval_run(code::AbstractString) figure() ax2=subplot(211) title("Commands") - plot(cmd_log.t,cmd_log.z,"-*",cmd_log.t_msg,cmd_log.z,"-x") + plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x") grid("on") xlabel("t [s]") subplot(212,sharex=ax2) - plot(cmd_pwm_log.t,cmd_pwm_log.z,"-*") + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z,"-*") grid("on") xlabel("t [s]") @@ -247,7 +247,6 @@ function eval_LMPC(code::AbstractString) x_est = d_lmpc["x_est"] coeffX = d_lmpc["coeffX"] coeffY = d_lmpc["coeffY"] - s_start = d_lmpc["s_start"] imu_meas = d_rec["imu_meas"] gps_meas = d_rec["gps_meas"] cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator @@ -287,7 +286,7 @@ function eval_LMPC(code::AbstractString) figure(4) title("Open loop predictions") #plot(t,state[:,[1,4,5]]) - for i=1:4:size(t,1)-10 + for i=1:1:size(t,1)-10 if sol_z[1,5,i]==0 plot(t[i:i+10]-t0,sol_z[:,2:4,i]) else @@ -298,7 +297,7 @@ function eval_LMPC(code::AbstractString) figure() title("Open loop inputs") - for i=1:4:size(t,1)-10 + for i=1:1:size(t,1)-10 plot(t[i:i+7]-t0,sol_u[1:8,2,i],t[i:i+9]-t0,sol_u[:,1,i]) end grid("on") @@ -318,23 +317,23 @@ function eval_LMPC(code::AbstractString) grid("on") # *********** CURVATURE ********************* - # figure() - # c = zeros(size(curv,1),1) - # for i=1:size(curv,1) - # s = state[i,6]-s_start[i] - # c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] - # end - # plot(s_start+state[:,1],c,"-o") - # for i=1:2:size(curv,1) - # s = sol_z[:,1,i] - # c = zeros(size(curv,1),1) - # c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - # plot(s_start[i]+s,c,"-*") - # end - # title("Curvature over path") - # xlabel("Curvilinear abscissa [m]") - # ylabel("Curvature") - # grid() + figure() + c = zeros(size(curv,1),1) + for i=1:size(curv,1) + s = state[i,6] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + end + plot(state[:,1],c,"-o") + for i=1:2:size(curv,1) + s = sol_z[:,1,i] + c = zeros(size(curv,1),1) + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + plot(s,c,"-*") + end + title("Curvature over path") + xlabel("Curvilinear abscissa [m]") + ylabel("Curvature") + grid() # track = create_track(0.3) # figure() @@ -383,12 +382,12 @@ function eval_LMPC(code::AbstractString) # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") # end - figure() - plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-x") - title("Old Trajectory") - legend(["v_x","v_y","psiDot","ePsi","eY"]) - xlabel("s") - grid(1) + # figure() + # plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-x") + # title("Old Trajectory") + # legend(["v_x","v_y","psiDot","ePsi","eY"]) + # xlabel("s") + # grid(1) figure() ax1=subplot(211) @@ -417,10 +416,18 @@ function eval_oldTraj(code::AbstractString,i::Int64) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" d = load(log_path_LMPC) oldTraj = d["oldTraj"] - t = d["t"] - plot(t,oldTraj[:,:,1,i],"-*") + #t = d["t"] + plot(oldTraj.oldTimes[:,i],oldTraj.oldTraj[:,:,i],"-x") grid("on") legend(["v_x","v_x","psiDot","ePsi","eY","s"]) + figure() + plot(oldTraj.oldTimes[:,i],oldTraj.oldInput[:,:,i],"-x") + grid("on") + legend(["a","d_f"]) + figure() + plot(oldTraj.oldTraj[:,6,i],oldTraj.oldTraj[:,1:5,i],"-x") + grid("on") + legend(["v_x","v_x","psiDot","ePsi","eY"]) end function eval_LMPC_coeff(code::AbstractString,k::Int64) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 15eca225..1140bf2b 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -19,24 +19,53 @@ include("barc_lib/LMPC/coeffConstraintCost.jl") include("barc_lib/LMPC/solveMpcProblem.jl") include("barc_lib/LMPC/functions.jl") -function SE_callback(msg::pos_info,lapStatus::LapStatus,oldTraj::OldTrajectory,coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, - coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data +# This function is called whenever a new state estimate is received. +# It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) +function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data # update mpc initial condition z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer coeffCurvature_update[:] = msg.coeffCurvature x_est[:] = [msg.x,msg.y,msg.psi,msg.v] - coeffX[:] = msg.coeffX - coeffY[:] = msg.coeffY + # check if lap needs to be switched + if z_est[6] % posInfo.s_target <= lapStatus.s_lapTrigger && lapStatus.switchLap + lapStatus.currentLap += 1 + lapStatus.nextLap = true + lapStatus.switchLap = false + elseif z_est[6] > lapStatus.s_lapTrigger + lapStatus.switchLap = true + end + # save current state in oldTraj - oldTraj[oldTraj.count,:,lapStatus.currentLap] = z_est - oldTraj.count += 1 + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = z_est + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = [mpcSol.a_x,mpcSol.d_f] + oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap],lapStatus.currentLap] = to_sec(msg.header.stamp) + oldTraj.count[lapStatus.currentLap] += 1 + + # if necessary: append to end of previous lap + if lapStatus.currentLap > 1 && z_est[6] < 3.0 + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [mpcSol.a_x,mpcSol.d_f] + oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap-1],lapStatus.currentLap-1] = to_sec(msg.header.stamp) + oldTraj.count[lapStatus.currentLap-1] += 1 + end + + #if necessary: append to beginning of next lap + if z_est[6] > posInfo.s_target - 3.0 + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [mpcSol.a_x,mpcSol.d_f] + oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap+1],lapStatus.currentLap+1] = to_sec(msg.header.stamp) + oldTraj.count[lapStatus.currentLap+1] += 1 + end end +# This is the main function, it is called when the node is started. function main() println("Starting LMPC node.") - buffersize = 1000 # size of oldTraj buffers + buffersize = 2000 # size of oldTraj buffers # Define and initialize variables # --------------------------------------------------------------- @@ -44,7 +73,7 @@ function main() oldTraj = OldTrajectory() posInfo = PosInfo() mpcCoeff = MpcCoeff() - lapStatus = LapStatus(1,1) + lapStatus = LapStatus(1,1,false,false,0.3) mpcSol = MpcSol() trackCoeff = TrackCoeff() # info about track (at current position, approximated) modelParams = ModelParams() @@ -83,20 +112,16 @@ function main() log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) log_step_diff = zeros(10000,5) - log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(10) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,oldTraj,lapStatus,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,posInfo,mpcSol,oldTraj,coeffCurvature_update,z_est,x_est),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} run_id = get_param("run_id") println("Finished initialization.") - # Lap parameters - switchLap = false # initialize lap lap trigger - s_lapTrigger = 0.3 # next lap is triggered in the interval s in [0,s_lapTrigger] # buffer in current lap zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) @@ -119,14 +144,18 @@ function main() oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) posInfo.s = posInfo.s_target/2 + lapStatus.currentLap = 3 + oldTraj.count[3] = 200 coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) - oldTraj.oldTraj[1:buffersize,6,1] = zeros(buffersize,1) - oldTraj.oldTraj[1:buffersize,6,2] = zeros(buffersize,1) + oldTraj.count[3] = 1 + lapStatus.currentLap = 1 + oldTraj.oldTraj[1:buffersize,6,1] = NaN*ones(buffersize,1) + oldTraj.oldTraj[1:buffersize,6,2] = NaN*ones(buffersize,1) posInfo.s = 0 uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - n_pf = 2 # number of first path-following laps (needs to be at least 2) + n_pf = 3 # number of first path-following laps (needs to be at least 2) # Start node while ! is_shutdown() @@ -147,7 +176,7 @@ function main() # ============================= Pre-Logging (before solving) ================================ log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) - if lapStatus.currentLap <= n_pf # find 1-step-error + if size(mpcSol.z,2) <= 4 # find 1-step-error step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]).^2 else step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 @@ -155,22 +184,22 @@ function main() log_step_diff[k+1,:] = step_diff # ======================================= Lap trigger ======================================= - if posInfo.s%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... + if lapStatus.nextLap # if we are switching to the next lap... println("Finishing one lap at iteration $i") println("current state: $(zCurr[i,:])") println("previous state: $(zCurr[i-1,:])") # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj - saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,posInfo,buffersize) - println("oldTraj: $(oldTraj.oldTraj[:,:,1])") - log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] zCurr[1,:] = zCurr[i,:] # copy current state u_final  = uCurr[i-1,:] # ... and input i = 1 lapStatus.currentIt = 1 # reset current iteration - lapStatus.currentLap += 1 # start next lap - switchLap = false - elseif posInfo.s%posInfo.s_target > s_lapTrigger - switchLap = true + lapStatus.nextLap = false + # Set warm start for new solution (because s shifted by s_target) + if lapStatus.currentLap <= n_pf + setvalue(mdl_pF.z_Ol[:,1],mpcSol.z[:,1]-posInfo.s_target) + elseif lapStatus.currentLap > n_pf+1 + setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + end end # ======================================= Calculate input ======================================= @@ -188,12 +217,6 @@ function main() println("Finished coefficients, t = $tt s") end - # Find last inputs u_i-1 - if i>1 # last u is needed for smooth MPC solutions (for derivative of u) - last_u = uCurr[i-1,:] - else - last_u = u_final - end println("Starting solving.") # Solve the MPC problem tic() @@ -202,7 +225,7 @@ function main() solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) else # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev[1,:]) end tt = toq() @@ -215,14 +238,14 @@ function main() println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") # append new states and inputs to old trajectory - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target - oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = uCurr[i,:] - if lapStatus.currentLap==3 # if its the third lap, append to both old trajectories! (since both are the same) - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = zCurr[i,:] - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,2] += posInfo.s_target - oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] - end + # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] + # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target + # oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = uCurr[i,:] + # if lapStatus.currentLap==3 # if its the third lap, append to both old trajectories! (since both are the same) + # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = zCurr[i,:] + # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,2] += posInfo.s_target + # oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] + # end # For System ID: Update last 50 measurements z_ID = circshift(z_ID,-1) @@ -264,7 +287,7 @@ function main() log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4])-2.jld" warn("Warning: File already exists.") end - save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], + save(log_path,"oldTraj",oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) diff --git a/workspace/src/barc/src/LMPC_node.jl.orig b/workspace/src/barc/src/LMPC_node.jl.orig new file mode 100755 index 00000000..5b952ea4 --- /dev/null +++ b/workspace/src/barc/src/LMPC_node.jl.orig @@ -0,0 +1,307 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/coeffConstraintCost.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/LMPC/functions.jl") + +<<<<<<< HEAD +function SE_callback(msg::pos_info,lapStatus::LapStatus,oldTraj::OldTrajectory,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, +======= +function SE_callback(msg::pos_info,coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, +>>>>>>> no-s-start + coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data + # update mpc initial condition + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer + coeffCurvature_update[:] = msg.coeffCurvature + x_est[:] = [msg.x,msg.y,msg.psi,msg.v] + coeffX[:] = msg.coeffX + coeffY[:] = msg.coeffY + + # save current state in oldTraj + oldTraj[oldTraj.count,:,lapStatus.currentLap] = z_est + oldTraj.count += 1 +end + +function main() + println("Starting LMPC node.") + + buffersize = 1000 # size of oldTraj buffers + + # Define and initialize variables + # --------------------------------------------------------------- + # General LMPC variables + oldTraj = OldTrajectory() + posInfo = PosInfo() + mpcCoeff = MpcCoeff() + lapStatus = LapStatus(1,1) + mpcSol = MpcSol() + trackCoeff = TrackCoeff() # info about track (at current position, approximated) + modelParams = ModelParams() + mpcParams = MpcParams() + mpcParams_pF = MpcParams() # for 1st lap (path following) + + InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) + + z_ID = zeros(50,6) + u_ID = zeros(50,2) + + # ROS-specific variables + z_est = zeros(6) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) + x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) + coeffX = zeros(9) # buffer for coeffX (only logging) + coeffY = zeros(9) # buffer for coeffY (only logging) + cmd = ECU() # command type + coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) + + # Logging variables + log_coeff_Cost = zeros(mpcCoeff.order+1,2,10000) + log_coeff_Const = zeros(mpcCoeff.order+1,2,5,10000) + log_sol_z = zeros(mpcParams.N+1,6,10000) + log_sol_u = zeros(mpcParams.N,2,10000) + log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) + log_state_x = zeros(10000,4) + log_coeffX = zeros(10000,9) + log_coeffY = zeros(10000,9) + log_t = zeros(10000,1) + log_state = zeros(10000,6) + log_cost = zeros(10000,6) + log_c_Vx = zeros(10000,3) + log_c_Vy = zeros(10000,4) + log_c_Psi = zeros(10000,3) + log_cmd = zeros(10000,2) + log_step_diff = zeros(10000,5) + log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps + + # Initialize ROS node and topics + init_node("mpc_traj") + loop_rate = Rate(10) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} +<<<<<<< HEAD + # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: + s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,oldTraj,s_start_update,lapStatus,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} +======= + # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: + s1 = Subscriber("pos_info", pos_info, SE_callback, (coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} +>>>>>>> no-s-start + + run_id = get_param("run_id") + println("Finished initialization.") + # Lap parameters + switchLap = false # initialize lap lap trigger + s_lapTrigger = 0.3 # next lap is triggered in the interval s in [0,s_lapTrigger] + + # buffer in current lap + zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) + uCurr = zeros(10000,2) # contains input information + u_final = zeros(2) # contains last input of one lap + step_diff = zeros(5) + + # Specific initializations: + lapStatus.currentLap = 1 + lapStatus.currentIt = 1 + posInfo.s_target = 17.76# 12.0#24.0 + k = 0 # overall counter for logging + + mpcSol.z = zeros(11,4) + mpcSol.u = zeros(10,2) + mpcSol.a_x = 0 + mpcSol.d_f = 0 + + # Precompile coeffConstraintCost: + oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) + oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) + posInfo.s = posInfo.s_target/2 + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) + oldTraj.oldTraj[1:buffersize,6,1] = zeros(buffersize,1) + oldTraj.oldTraj[1:buffersize,6,2] = zeros(buffersize,1) + posInfo.s = 0 + + uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) + + n_pf = 2 # number of first path-following laps (needs to be at least 2) + + # Start node + while ! is_shutdown() + if z_est[6] > 0 # check if data has been received (s > 0) + # ============================= PUBLISH COMMANDS ============================= + # This is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received + # This guarantees a constant publishing frequency of 10 Hz + # (The state can be predicted by 0.1s) + cmd.header.stamp = get_rostime() + cmd.motor = convert(Float32,mpcSol.a_x) + cmd.servo = convert(Float32,mpcSol.d_f) + publish(pub, cmd) + # ============================= Initialize iteration parameters ============================= + i = lapStatus.currentIt # current iteration number, just to make notation shorter + zCurr[i,:] = copy(z_est) # update state information + posInfo.s = zCurr[i,6] # update position info + trackCoeff.coeffCurvature = copy(coeffCurvature_update) + + # ============================= Pre-Logging (before solving) ================================ + log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) + if lapStatus.currentLap <= n_pf # find 1-step-error + step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]).^2 + else + step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 + end + log_step_diff[k+1,:] = step_diff + + # ======================================= Lap trigger ======================================= + if posInfo.s%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... + println("Finishing one lap at iteration $i") + println("current state: $(zCurr[i,:])") + println("previous state: $(zCurr[i-1,:])") + # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj + saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,posInfo,buffersize) + println("oldTraj: $(oldTraj.oldTraj[:,:,1])") + log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] + zCurr[1,:] = zCurr[i,:] # copy current state + u_final  = uCurr[i-1,:] # ... and input + i = 1 + lapStatus.currentIt = 1 # reset current iteration + lapStatus.currentLap += 1 # start next lap + switchLap = false + elseif posInfo.s%posInfo.s_target > s_lapTrigger + switchLap = true + end + + # ======================================= Calculate input ======================================= + println("=================================== NEW ITERATION # $i ===================================") + println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") + println("State Nr. $i = $z_est") + println("s = $(posInfo.s)") + println("s_total = $(posInfo.s%posInfo.s_target)") + + # Find coefficients for cost and constraints + if lapStatus.currentLap > n_pf + tic() + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) + tt = toq() + println("Finished coefficients, t = $tt s") + end + + # Find last inputs u_i-1 + if i>1 # last u is needed for smooth MPC solutions (for derivative of u) + last_u = uCurr[i-1,:] + else + last_u = u_final + end + println("Starting solving.") + # Solve the MPC problem + tic() + if lapStatus.currentLap <= n_pf + z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states + solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) + else # otherwise: use system-ID-model + #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') + end + tt = toq() + + # Write current input information + uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] + zCurr[i,6] = posInfo.s%posInfo.s_target # save absolute position in s (for oldTrajectory) + + uPrev = circshift(uPrev,1) + uPrev[1,:] = uCurr[i,:] + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") + + # append new states and inputs to old trajectory + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target + oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = uCurr[i,:] + if lapStatus.currentLap==3 # if its the third lap, append to both old trajectories! (since both are the same) + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = zCurr[i,:] + oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,2] += posInfo.s_target + oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] + end + + # For System ID: Update last 50 measurements + z_ID = circshift(z_ID,-1) + u_ID = circshift(u_ID,-1) + z_ID[end,:] = zCurr[i,:] + u_ID[end,:] = uCurr[i,:] + + # Logging + # --------------------------- + k = k + 1 # counter + log_state[k,:] = zCurr[i,:] + log_cmd[k+1,:] = uCurr[i,:] # the command is going to be pubished in the next iteration + log_sol_u[:,:,k] = mpcSol.u + log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost + log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst + log_cost[k,:] = mpcSol.cost + log_curv[k,:] = trackCoeff.coeffCurvature + log_state_x[k,:] = x_est + log_c_Vx[k,:] = mpcCoeff.c_Vx + log_c_Vy[k,:] = mpcCoeff.c_Vy + log_c_Psi[k,:] = mpcCoeff.c_Psi + if lapStatus.currentLap <= n_pf + log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) + else + log_sol_z[:,1:6,k] = mpcSol.z + end + + # Count one up: + lapStatus.currentIt += 1 + else + println("No estimation data received!") + end + rossleep(loop_rate) + end + # Save simulation data to file + + log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4]).jld" + if isfile(log_path) + log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4])-2.jld" + warn("Warning: File already exists.") + end + save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], + "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, + "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], + "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) + println("Exiting LMPC node. Saved data to $log_path.") + +end + +if ! isinteractive() + main() +end + +# Sequence within one iteration: +# 1. Publish commands from last iteration (because the car is in real *now* where we thought it was before (predicted state)) +# 2. Receive new state information +# 3. Check if we've crossed the finish line and if we have, switch lap number and save old trajectories +# 4. (If in 3rd lap): Calculate coefficients +# 5. Calculate MPC commands (depending on lap) which are going to be published in the next iteration +# 6. (Do some logging) + + +# Definitions of variables: +# zCurr contains all state information from the beginning of the lap (first s >= 0) to the current state i +# uCurr -> same as zCurr for inputs +# generally: zCurr[i+1] = f(zCurr[i],uCurr[i]) + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 3c683d82..78b9f198 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 3c683d826069bb7fdc2476705e6a26acb21e38a3 +Subproject commit 78b9f1989884673b3f5f850637f59ccf94218168 diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py index d658d4b9..ed4d6d91 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py @@ -158,7 +158,7 @@ def state_estimation(): P = eye(6) # initial dynamics coveriance matrix Q = diag([0.1,0.1,1.0,0.1,0.1,0.01]) - R = diag([1.0,1.0,0.1,0.1,5.0]) + R = diag([1.0,1.0,1.0,1.0,5.0]) # Set up track parameters l = Localization() @@ -166,6 +166,8 @@ def state_estimation(): l.prepare_trajectory(0.06) d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_f_lp = 0 + a_lp = 0 # Estimation variables (x_est, y_est) = [0]*2 @@ -185,16 +187,16 @@ def state_estimation(): se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) R[0,0] = 100.0 R[1,1] = 100.0 - if se.imu_updated: - R[3,3] = 1.0 - R[4,4] = 5.0 - else: - R[3,3] = 10.0 - R[4,4] = 50.0 - if se.vel_updated: - R[2,2] = 0.1 - else: - R[2,2] = 1.0 + # if se.imu_updated: + # R[3,3] = 1.0 + # R[4,4] = 5.0 + # else: + # R[3,3] = 10.0 + # R[4,4] = 50.0 + # if se.vel_updated: + # R[2,2] = 0.1 + # else: + # R[2,2] = 1.0 se.gps_updated = False se.imu_updated = False @@ -203,8 +205,11 @@ def state_estimation(): y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas]) # define input - d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering - u = [se.cmd_motor, d_f_hist.pop(0)] + #d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering + a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) + #u = [se.cmd_motor, d_f_hist.pop(0)] + u = [a_lp, d_f_lp] # build extra arguments for non-linear function args = (u, vhMdl, dt, est_mode) From 4aabff9bb003d8adcbf80c3c804b8eb0a572903d Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 11 Nov 2016 10:50:57 -0800 Subject: [PATCH 087/183] Added pure sensor-based Kalman filter (good performance in tests), ran first successful sysID laps. --- eval_sim/Kalman simulation/sim_kalman_imu.jl | 150 +++++---- eval_sim/Other tests/NormalizeImu.jl | 44 +++ .../Parameter estimation/find_a_mapping.jl | 69 ++++ eval_sim/eval_data.jl | 42 +++ workspace/src/barc/launch/barc_sim.launch | 3 +- .../src/barc/launch/open_loop_remote.launch | 3 +- .../src/barc/launch/run_lmpc_remote.launch | 3 +- workspace/src/barc/msg/pos_info.msg | 2 + workspace/src/barc/src/LMPC_node.jl | 23 +- workspace/src/barc/src/LMPC_node.jl.orig | 307 ------------------ workspace/src/barc/src/barc_record.jl | 2 +- workspace/src/barc/src/open_loop.jl | 60 ++-- .../src/state_estimation_KinBkMdl_mixed.py | 2 +- ...ift.py => state_estimation_SensorModel.py} | 93 ++++-- workspace/src/barc/src/system_models.py | 107 +----- 15 files changed, 385 insertions(+), 525 deletions(-) create mode 100644 eval_sim/Other tests/NormalizeImu.jl create mode 100644 eval_sim/Parameter estimation/find_a_mapping.jl delete mode 100755 workspace/src/barc/src/LMPC_node.jl.orig rename workspace/src/barc/src/{state_estimation_DynBkMdl_mixed_nodrift.py => state_estimation_SensorModel.py} (69%) diff --git a/eval_sim/Kalman simulation/sim_kalman_imu.jl b/eval_sim/Kalman simulation/sim_kalman_imu.jl index 121b7703..57f86b68 100644 --- a/eval_sim/Kalman simulation/sim_kalman_imu.jl +++ b/eval_sim/Kalman simulation/sim_kalman_imu.jl @@ -4,30 +4,26 @@ using PyPlot using JLD -Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) -R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) - -R_gps_imu = diagm([0.1,0.1,1.0,1.0,0.1,0.1,0.01]) function main(code::AbstractString) - global Q, R, R_gps_imu - log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) imu_meas = d_rec["imu_meas"] gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - cmd_pwm_log = d_rec["cmd_pwm_log"] + cmd_log = d_rec["cmd_pwm_log"] + #cmd_pwm_log = d_rec["cmd_pwm_log"] vel_est = d_rec["vel_est"] pos_info = d_rec["pos_info"] - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) l_A = 0.125 l_B = 0.125 - dt = 1/25 + dt = 1/50 t = t0:dt:t_end sz = length(t) y = zeros(sz,6) @@ -37,71 +33,92 @@ function main(code::AbstractString) P = zeros(7,7) x_est = zeros(length(t),7) - P_gps_imu = zeros(11,11) - x_est_gps_imu = zeros(length(t),11) + P_gps_imu = zeros(9,9) + x_est_gps_imu = zeros(length(t),9) - yaw0 = imu_meas.z[1,6] + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] gps_dist = zeros(length(t)) yaw_prev = yaw0 + y_gps_imu[1,4] = 0 - Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + # x, y, v, psi, psidot, ax, ay for i=2:length(t) # Collect measurements and inputs for this iteration - y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] - y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 - y_yawdot = imu_meas.z[imu_meas.t.>t[i],3][1] - a_x = imu_meas.z[imu_meas.t.>t[i],7][1] - a_y = imu_meas.z[imu_meas.t.>t[i],8][1] - y_vel_est = vel_est.z[vel_est.t.>t[i]][1] - - y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] - y_gps_imu[i,:] = [y_gps a_x a_y y_yaw y_yawdot y_vel_est] - y[:,3] = unwrap!(y[:,3]) - y_gps_imu[:,5] = unwrap!(y_gps_imu[:,5]) + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] + #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + + att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] + acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' + #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = acc_f[1] + a_y = acc_f[2] + + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] # Adapt R-value of GPS according to distance to last point: gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) if gps_dist[i] > 0.3 - R[1,1] = 1+100*gps_dist[i]^2 - R[2,2] = 1+100*gps_dist[i]^2 - R_gps_imu[1,1] = 1+100*gps_dist[i]^2 - R_gps_imu[2,2] = 1+100*gps_dist[i]^2 + R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 + R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 else - R[1,1] = 1 - R[2,2] = 1 R_gps_imu[1,1] = 1 R_gps_imu[2,2] = 1 end - args = (u[i,:],dt,l_A,l_B) # Calculate new estimate - (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) (x_est_gps_imu[i,:], P_gps_imu) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) end + #figure(5) + #plot(t-t0,y_gps_imu) + #grid("on") + + println("Yaw0 = $yaw0") figure(1) plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) + plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") grid("on") + legend(["x_est","y_est","x_meas","y_meas"]) title("Comparison x,y estimate and measurement") figure(2) - plot(t-t0,x_est_gps_imu[:,9:11]) + plot(t-t0,x_est_gps_imu[:,[7,9]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) + plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") grid("on") - title("Drifts") - legend(["a_x","a_y","psi"]) + legend(["psi_est","psi_drift_est","psi_meas"]) figure(3) v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") + legend(["v_est","v_x_est","v_y_est","v_meas"]) grid("on") title("Velocity estimate and measurement") + figure(4) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) + #plot(pos_info.t-t0,pos_info.z[:,17:18]) + grid("on") + legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + # ax1=subplot(211) # plot(t,y,"-x",t,x_est,"-*") # grid("on") @@ -181,19 +198,16 @@ function h(x,args) return C*x end function h_gps_imu(x,args) - C = [1 0 0 0 0 0 0 0 0 0 0; # x - 0 1 0 0 0 0 0 0 0 0 0; # y - 0 0 0 0 1 0 0 0 1 0 0; # a_x - 0 0 0 0 0 1 0 0 0 1 0; # a_y - 0 0 0 0 0 0 1 0 0 0 1; # psi - 0 0 0 0 0 0 0 1 0 0 0; # psi_dot - 0 0 0 0 0 0 0 0 0 0 0] # v - y = C*x - y[7] = sqrt(x[3]^2+x[4]^2) # Idea: v_x_meas = cos(beta)*v, v_y_meas = sin(beta)*v - #return C*x + y = zeros(7) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[9] # psi + y[5] = x[8] # psiDot + y[6] = x[5] # a_x + y[7] = x[6] # a_y return y end - function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) xDim = size(mx_k,1) mx_kp1 = f(mx_k,args) @@ -232,20 +246,18 @@ function simModel_gps_imu(z,args) zNext = copy(z) (u,dt,l_A,l_B) = args bta = atan(l_A/(l_A+l_B)*tan(u[2])) - zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x - zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y - #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*(z[5]+z[8]*z[4])-sin(z[7])*(z[6]-z[8]*z[3])) # x - #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*(z[5]+z[8]*z[4])+cos(z[7])*(z[6]-z[8]*z[3])) # y + #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x + #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y + zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x + zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y zNext[5] = z[5] # a_x zNext[6] = z[6] # a_y - zNext[7] = z[7] + dt*(z[3]/l_B*sin(bta)) # psi - #zNext[7] = z[7] + dt*z[8] # psi + #zNext[7] = z[7] + dt*(z[3]/l_B*sin(bta)) # psi + zNext[7] = z[7] + dt*z[8] # psi zNext[8] = z[8] # psidot - zNext[9] = z[9] # drift_a_x - zNext[10] = z[10] # drift_a_y - zNext[11] = z[11] # drift_psi + zNext[9] = z[9] # drift_psi return zNext end @@ -291,4 +303,24 @@ function unwrap!(p) end end return p -end \ No newline at end of file +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end diff --git a/eval_sim/Other tests/NormalizeImu.jl b/eval_sim/Other tests/NormalizeImu.jl new file mode 100644 index 00000000..b3f6c0b9 --- /dev/null +++ b/eval_sim/Other tests/NormalizeImu.jl @@ -0,0 +1,44 @@ +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + t0 = imu_meas.t[1] + sz = size(imu_meas.t,1) + a_norm = zeros(sz,3) + for i=1:sz + a_norm[i,:] = (rotMatrix('y',-imu_meas.z[i,5])*rotMatrix('x',-imu_meas.z[i,4])*imu_meas.z[i,7:9]')' + end + figure() + plot(imu_meas.t-t0,imu_meas.z[:,7:8]) + grid("on") + figure() + plot(imu_meas.t-t0,a_norm[:,1:2]) + grid("on") + println("Mean raw: $(mean(imu_meas.z[:,8])), std raw: $(std(imu_meas.z[:,8]))") + println("Mean filtered: $(mean(a_norm[:,2])), std filtered: $(std(a_norm[:,2]))") +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end diff --git a/eval_sim/Parameter estimation/find_a_mapping.jl b/eval_sim/Parameter estimation/find_a_mapping.jl new file mode 100644 index 00000000..18ff44b2 --- /dev/null +++ b/eval_sim/Parameter estimation/find_a_mapping.jl @@ -0,0 +1,69 @@ + +using PyPlot +using JLD + +# type Measurements{T} +# i::Int64 # measurement counter +# t::Array{Float64} # time data (when it was received by this recorder) +# t_msg::Array{Float64} # time that the message was sent +# z::Array{T} # measurement values +# end + +function main(code::AbstractString) + #log_path_record = ("$(homedir())/open_loop/output-record-8bbb.jld","$(homedir())/open_loop/output-record-3593.jld") + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + v_opt = v[500:end] + cmd_opt = cmd[500:end] + + res = optimize(c->cost(cmd_opt,v_opt,c,0),[0.001,0,0.1,0.001,0]) + c = Optim.minimizer(res) + c2 = [1/c[1], c[2]/c[1], 1/c[4], c[5]/c[4]] + cost(cmd_opt,v_opt,c,1) + println("c = $c") + println("and c_2 = $c2") +end + +function cost(cmd,v,c,p) + cost = 0 + v_s = zeros(length(v)) + v_s[1] = v[1] + v_s[2] = v[2] + for i=2:length(cmd)-1 + if cmd[i]-cmd[i-1] != 0 + v_s[i] = v[i] + end + if cmd[i] > 90 + v_s[i+1] = v_s[i] + 0.02*(c[1]*cmd[i] + c[2] - c[3]*v_s[i]) + else + v_s[i+1] = max(v_s[i] + 0.02*(c[4]*cmd[i] + c[5] - c[3]*v_s[i]),0) + end + end + if p==1 + plot((1:length(v_s))*0.02,v_s,(1:length(v))*0.02,v) + grid("on") + legend(["v_sim","v_meas"]) + title("Curve fitting acceleration") + xlabel("t [s]") + ylabel("v [m/s]") + end + cost = norm(v_s-v) + return cost +end diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 938710c1..88ab2cd9 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -17,6 +17,8 @@ using HDF5, JLD, ProfileView # pos_info[14] = psi_raw # pos_info[15] = v_raw # pos_info[16] = psi_drift +# pos_info[17] = a_x +# pos_info[18] = a_y include("../workspace/src/barc/src/barc_lib/classes.jl") @@ -412,6 +414,46 @@ function eval_LMPC(code::AbstractString) legend(["u","d_f"]) end +function eval_sysID(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + + t0 = imu_meas.t[1] + + figure(1) # longitudinal (xDot) + ax1=subplot(211) + plot(t-t0,c_Vx) + legend(["c1","c2","c3"]) + grid("on") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + grid("on") +end + + function eval_oldTraj(code::AbstractString,i::Int64) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" d = load(log_path_LMPC) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index e2e96863..4a141a5a 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -18,7 +18,8 @@ - + + diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 4abac3ea..ab15063e 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -13,7 +13,8 @@ - + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index f1b1133d..859371fa 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -17,7 +17,8 @@ - + + diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index bfbf4811..bad43795 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -17,6 +17,8 @@ float64 y_raw float64 psi_raw float64 v_raw float64 psi_drift +float64 a_x +float64 a_y float64[] coeffX float64[] coeffY float64[] coeffTheta diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 1140bf2b..0a133560 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -84,9 +84,6 @@ function main() mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) - z_ID = zeros(50,6) - u_ID = zeros(50,2) - # ROS-specific variables z_est = zeros(6) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) @@ -146,7 +143,7 @@ function main() posInfo.s = posInfo.s_target/2 lapStatus.currentLap = 3 oldTraj.count[3] = 200 - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) oldTraj.count[3] = 1 lapStatus.currentLap = 1 oldTraj.oldTraj[1:buffersize,6,1] = NaN*ones(buffersize,1) @@ -212,7 +209,7 @@ function main() # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) tt = toq() println("Finished coefficients, t = $tt s") end @@ -237,22 +234,6 @@ function main() uPrev[1,:] = uCurr[i,:] println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") - # append new states and inputs to old trajectory - # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] - # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target - # oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = uCurr[i,:] - # if lapStatus.currentLap==3 # if its the third lap, append to both old trajectories! (since both are the same) - # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = zCurr[i,:] - # oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,2] += posInfo.s_target - # oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] - # end - - # For System ID: Update last 50 measurements - z_ID = circshift(z_ID,-1) - u_ID = circshift(u_ID,-1) - z_ID[end,:] = zCurr[i,:] - u_ID[end,:] = uCurr[i,:] - # Logging # --------------------------- k = k + 1 # counter diff --git a/workspace/src/barc/src/LMPC_node.jl.orig b/workspace/src/barc/src/LMPC_node.jl.orig deleted file mode 100755 index 5b952ea4..00000000 --- a/workspace/src/barc/src/LMPC_node.jl.orig +++ /dev/null @@ -1,307 +0,0 @@ -#!/usr/bin/env julia - -using RobotOS -@rosimport barc.msg: ECU, pos_info -@rosimport data_service.msg: TimeData -@rosimport geometry_msgs.msg: Vector3 -rostypegen() -using barc.msg -using data_service.msg -using geometry_msgs.msg -using JuMP -using Ipopt -using JLD - -# log msg -include("barc_lib/classes.jl") -include("barc_lib/LMPC/MPC_models.jl") -include("barc_lib/LMPC/coeffConstraintCost.jl") -include("barc_lib/LMPC/solveMpcProblem.jl") -include("barc_lib/LMPC/functions.jl") - -<<<<<<< HEAD -function SE_callback(msg::pos_info,lapStatus::LapStatus,oldTraj::OldTrajectory,s_start_update::Array{Float64},coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, -======= -function SE_callback(msg::pos_info,coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}, ->>>>>>> no-s-start - coeffX::Array{Float64,1},coeffY::Array{Float64,1}) # update current position and track data - # update mpc initial condition - z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer - coeffCurvature_update[:] = msg.coeffCurvature - x_est[:] = [msg.x,msg.y,msg.psi,msg.v] - coeffX[:] = msg.coeffX - coeffY[:] = msg.coeffY - - # save current state in oldTraj - oldTraj[oldTraj.count,:,lapStatus.currentLap] = z_est - oldTraj.count += 1 -end - -function main() - println("Starting LMPC node.") - - buffersize = 1000 # size of oldTraj buffers - - # Define and initialize variables - # --------------------------------------------------------------- - # General LMPC variables - oldTraj = OldTrajectory() - posInfo = PosInfo() - mpcCoeff = MpcCoeff() - lapStatus = LapStatus(1,1) - mpcSol = MpcSol() - trackCoeff = TrackCoeff() # info about track (at current position, approximated) - modelParams = ModelParams() - mpcParams = MpcParams() - mpcParams_pF = MpcParams() # for 1st lap (path following) - - InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) - mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) - mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) - - z_ID = zeros(50,6) - u_ID = zeros(50,2) - - # ROS-specific variables - z_est = zeros(6) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) - x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) - coeffX = zeros(9) # buffer for coeffX (only logging) - coeffY = zeros(9) # buffer for coeffY (only logging) - cmd = ECU() # command type - coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) - - # Logging variables - log_coeff_Cost = zeros(mpcCoeff.order+1,2,10000) - log_coeff_Const = zeros(mpcCoeff.order+1,2,5,10000) - log_sol_z = zeros(mpcParams.N+1,6,10000) - log_sol_u = zeros(mpcParams.N,2,10000) - log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) - log_state_x = zeros(10000,4) - log_coeffX = zeros(10000,9) - log_coeffY = zeros(10000,9) - log_t = zeros(10000,1) - log_state = zeros(10000,6) - log_cost = zeros(10000,6) - log_c_Vx = zeros(10000,3) - log_c_Vy = zeros(10000,4) - log_c_Psi = zeros(10000,3) - log_cmd = zeros(10000,2) - log_step_diff = zeros(10000,5) - log_oldTraj = zeros(buffersize,6,2,20) # max. 20 laps - - # Initialize ROS node and topics - init_node("mpc_traj") - loop_rate = Rate(10) - pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} -<<<<<<< HEAD - # The subscriber passes arguments (s_start, coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,oldTraj,s_start_update,lapStatus,coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} -======= - # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (coeffCurvature_update,z_est,x_est,coeffX,coeffY,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} ->>>>>>> no-s-start - - run_id = get_param("run_id") - println("Finished initialization.") - # Lap parameters - switchLap = false # initialize lap lap trigger - s_lapTrigger = 0.3 # next lap is triggered in the interval s in [0,s_lapTrigger] - - # buffer in current lap - zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) - uCurr = zeros(10000,2) # contains input information - u_final = zeros(2) # contains last input of one lap - step_diff = zeros(5) - - # Specific initializations: - lapStatus.currentLap = 1 - lapStatus.currentIt = 1 - posInfo.s_target = 17.76# 12.0#24.0 - k = 0 # overall counter for logging - - mpcSol.z = zeros(11,4) - mpcSol.u = zeros(10,2) - mpcSol.a_x = 0 - mpcSol.d_f = 0 - - # Precompile coeffConstraintCost: - oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) - oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) - posInfo.s = posInfo.s_target/2 - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) - oldTraj.oldTraj[1:buffersize,6,1] = zeros(buffersize,1) - oldTraj.oldTraj[1:buffersize,6,2] = zeros(buffersize,1) - posInfo.s = 0 - - uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - - n_pf = 2 # number of first path-following laps (needs to be at least 2) - - # Start node - while ! is_shutdown() - if z_est[6] > 0 # check if data has been received (s > 0) - # ============================= PUBLISH COMMANDS ============================= - # This is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received - # This guarantees a constant publishing frequency of 10 Hz - # (The state can be predicted by 0.1s) - cmd.header.stamp = get_rostime() - cmd.motor = convert(Float32,mpcSol.a_x) - cmd.servo = convert(Float32,mpcSol.d_f) - publish(pub, cmd) - # ============================= Initialize iteration parameters ============================= - i = lapStatus.currentIt # current iteration number, just to make notation shorter - zCurr[i,:] = copy(z_est) # update state information - posInfo.s = zCurr[i,6] # update position info - trackCoeff.coeffCurvature = copy(coeffCurvature_update) - - # ============================= Pre-Logging (before solving) ================================ - log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) - if lapStatus.currentLap <= n_pf # find 1-step-error - step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]).^2 - else - step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 - end - log_step_diff[k+1,:] = step_diff - - # ======================================= Lap trigger ======================================= - if posInfo.s%posInfo.s_target <= s_lapTrigger && switchLap # if we are switching to the next lap... - println("Finishing one lap at iteration $i") - println("current state: $(zCurr[i,:])") - println("previous state: $(zCurr[i-1,:])") - # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj - saveOldTraj(oldTraj,zCurr,uCurr,lapStatus,posInfo,buffersize) - println("oldTraj: $(oldTraj.oldTraj[:,:,1])") - log_oldTraj[:,:,:,lapStatus.currentLap] = oldTraj.oldTraj[:,:,:] - zCurr[1,:] = zCurr[i,:] # copy current state - u_final  = uCurr[i-1,:] # ... and input - i = 1 - lapStatus.currentIt = 1 # reset current iteration - lapStatus.currentLap += 1 # start next lap - switchLap = false - elseif posInfo.s%posInfo.s_target > s_lapTrigger - switchLap = true - end - - # ======================================= Calculate input ======================================= - println("=================================== NEW ITERATION # $i ===================================") - println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") - println("State Nr. $i = $z_est") - println("s = $(posInfo.s)") - println("s_total = $(posInfo.s%posInfo.s_target)") - - # Find coefficients for cost and constraints - if lapStatus.currentLap > n_pf - tic() - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,z_ID,u_ID,lapStatus) - tt = toq() - println("Finished coefficients, t = $tt s") - end - - # Find last inputs u_i-1 - if i>1 # last u is needed for smooth MPC solutions (for derivative of u) - last_u = uCurr[i-1,:] - else - last_u = u_final - end - println("Starting solving.") - # Solve the MPC problem - tic() - if lapStatus.currentLap <= n_pf - z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states - solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) - else # otherwise: use system-ID-model - #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',last_u') - end - tt = toq() - - # Write current input information - uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] - zCurr[i,6] = posInfo.s%posInfo.s_target # save absolute position in s (for oldTrajectory) - - uPrev = circshift(uPrev,1) - uPrev[1,:] = uCurr[i,:] - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") - - # append new states and inputs to old trajectory - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = zCurr[i,:] - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,1] += posInfo.s_target - oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,1] = uCurr[i,:] - if lapStatus.currentLap==3 # if its the third lap, append to both old trajectories! (since both are the same) - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = zCurr[i,:] - oldTraj.oldTraj[oldTraj.oldCost[1]+oldTraj.prebuf+i,6,2] += posInfo.s_target - oldTraj.oldInput[oldTraj.oldCost[1]+oldTraj.prebuf+i,:,2] = uCurr[i,:] - end - - # For System ID: Update last 50 measurements - z_ID = circshift(z_ID,-1) - u_ID = circshift(u_ID,-1) - z_ID[end,:] = zCurr[i,:] - u_ID[end,:] = uCurr[i,:] - - # Logging - # --------------------------- - k = k + 1 # counter - log_state[k,:] = zCurr[i,:] - log_cmd[k+1,:] = uCurr[i,:] # the command is going to be pubished in the next iteration - log_sol_u[:,:,k] = mpcSol.u - log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost - log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst - log_cost[k,:] = mpcSol.cost - log_curv[k,:] = trackCoeff.coeffCurvature - log_state_x[k,:] = x_est - log_c_Vx[k,:] = mpcCoeff.c_Vx - log_c_Vy[k,:] = mpcCoeff.c_Vy - log_c_Psi[k,:] = mpcCoeff.c_Psi - if lapStatus.currentLap <= n_pf - log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) - else - log_sol_z[:,1:6,k] = mpcSol.z - end - - # Count one up: - lapStatus.currentIt += 1 - else - println("No estimation data received!") - end - rossleep(loop_rate) - end - # Save simulation data to file - - log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4]).jld" - if isfile(log_path) - log_path = "$(homedir())/simulations/output-LMPC-$(run_id[1:4])-2.jld" - warn("Warning: File already exists.") - end - save(log_path,"oldTraj",log_oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], - "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, - "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], - "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) - println("Exiting LMPC node. Saved data to $log_path.") - -end - -if ! isinteractive() - main() -end - -# Sequence within one iteration: -# 1. Publish commands from last iteration (because the car is in real *now* where we thought it was before (predicted state)) -# 2. Receive new state information -# 3. Check if we've crossed the finish line and if we have, switch lap number and save old trajectories -# 4. (If in 3rd lap): Calculate coefficients -# 5. Calculate MPC commands (depending on lap) which are going to be published in the next iteration -# 6. (Do some logging) - - -# Definitions of variables: -# zCurr contains all state information from the beginning of the lap (first s >= 0) to the current state i -# uCurr -> same as zCurr for inputs -# generally: zCurr[i+1] = f(zCurr[i],uCurr[i]) - -# zCurr[1] = v_x -# zCurr[2] = v_y -# zCurr[3] = psiDot -# zCurr[4] = ePsi -# zCurr[5] = eY -# zCurr[6] = s \ No newline at end of file diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index bf474556..bbfd06db 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -34,7 +34,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,16)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,18)) # initiate node, set up publisher / subscriber topics init_node("barc_record") diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 590cf72d..03590bbb 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -25,7 +25,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,16)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,18)) # Initialize ROS node and topics init_node("mpc_traj") @@ -60,12 +60,32 @@ function main() chicane_speed = 1.0 chicane_turn = -0.3 + + cmd_m = 93 + t_next = 8 + mode = 1 # 1 = acc, 2 = break, 3 = break + # Idea: 5 seconds acc, 3 seconds breaking. + # Start node - while t < 29.0 # run exactly x seconds + while ! is_shutdown()#t < 29.0 # run exactly x seconds t = to_sec(get_rostime())-t0 if t <= 3 - cmd.motor = 0.0 - cmd.servo = 0.0 + cmd.motor = 90.0 + cmd.servo = 80.0 + elseif t <= t_next + if mode == 1 + cmd.motor = cmd_m + elseif mode == 2 + cmd.motor = 80 + end + elseif t <= 300 + if mode == 1 + cmd_m += 1 + mode = 2 + else + mode = 1 + end + t_next = t + 6 # CHICANE: # elseif t <= 3 # cmd.motor = chicane_speed @@ -98,21 +118,21 @@ function main() # elseif t <= 123 # cmd.motor = 0.0+(t-3)/20 # cmd.servo = 0.0#-(t-3.0)/300-0.15 - elseif t <= 8 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 100.0 - elseif t <= 13 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 105.0 - elseif t <= 18 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 110.0 - elseif t <= 23 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 115.0 - elseif t <= 28 # CHECK TIME AND ACCELERATION !!! - cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - cmd.servo = 120.0 + # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = 100.0 + # elseif t <= 13 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = 105.0 + # elseif t <= 18 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = 110.0 + # elseif t <= 23 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = 115.0 + # elseif t <= 28 # CHECK TIME AND ACCELERATION !!! + # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! + # cmd.servo = 120.0 # elseif t <= 33 # CHECK TIME AND ACCELERATION !!! # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! # cmd.servo = -0.4 @@ -126,7 +146,7 @@ function main() # cmd.motor = -2.0 # cmd.servo = 0 else - cmd.motor = -1.0 + cmd.motor = 70.0 cmd.servo = 90.0 end cmd.header.stamp = get_rostime() diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py index ed4d6d91..aec42847 100755 --- a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py +++ b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py @@ -235,7 +235,7 @@ def state_estimation(): ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - l.coeffX.tolist(), l.coeffY.tolist(), + 0, 0, l.coeffX.tolist(), l.coeffY.tolist(), l.coeffTheta.tolist(), l.coeffCurvature.tolist())) # wait diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py b/workspace/src/barc/src/state_estimation_SensorModel.py similarity index 69% rename from workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py rename to workspace/src/barc/src/state_estimation_SensorModel.py index 82aea247..007cd721 100755 --- a/workspace/src/barc/src/state_estimation_DynBkMdl_mixed_nodrift.py +++ b/workspace/src/barc/src/state_estimation_SensorModel.py @@ -19,10 +19,11 @@ from sensor_msgs.msg import Imu from marvelmind_nav.msg import hedge_pos from std_msgs.msg import Header -from numpy import eye, array, zeros, diag, unwrap +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin from observers import ekf -from system_models import f_KinBkMdl_mixed_nodrift, h_KinBkMdl_mixed_nodrift +from system_models import f_SensorModel, h_SensorModel from tf import transformations +import math # ***_meas are values that are used by the Kalman filters # ***_raw are raw values coming from the sensors @@ -33,6 +34,7 @@ class StateEst(object): # input variables cmd_servo = 0 cmd_motor = 0 + cmd_t = 0 # IMU yaw_prev = 0 @@ -41,13 +43,18 @@ class StateEst(object): psiDot_meas = 0 a_x_meas = 0 a_y_meas = 0 + imu_updated = False # Velocity vel_meas = 0 + vel_updated = False + vel_prev = 0 + vel_count = 0 # this counts how often the same vel measurement has been received # GPS x_meas = 0 y_meas = 0 + gps_updated = False # General variables t0 = 0 # Time when the estimator was started @@ -60,7 +67,7 @@ def __init__(self): def ecu_callback(self, data): self.cmd_motor = data.motor # input motor force [N] self.cmd_servo = data.servo # input steering angle [rad] - if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero self.running = True # ultrasound gps data @@ -69,6 +76,7 @@ def gps_callback(self, data): #current_t = rospy.get_rostime().to_sec() self.x_meas = data.x_m self.y_meas = data.y_m + self.gps_updated = True # imu measurement update def imu_callback(self, data): @@ -101,10 +109,20 @@ def imu_callback(self, data): self.psiDot_meas = w_z self.a_x_meas = a_x self.a_y_meas = a_y + self.imu_updated = True def vel_est_callback(self, data): - if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement + #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est + if data.vel_est != self.vel_prev: self.vel_meas = data.vel_est + self.vel_updated = True + self.vel_prev = data.vel_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True # state estimation node def state_estimation(): @@ -139,56 +157,85 @@ def state_estimation(): z_EKF = zeros(9) # x, y, psi, v, psi_drift P = eye(9) # initial dynamics coveriance matrix - Q = diag([0.01,0.01,0.1,0.1,0.1,0.1,0.01,0.1,0.0000001]) - R = diag([0.1,0.1,0.1,0.1,0.01,1.0,1.0]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + Q = diag([0.01,0.01,0.1,0.1,1.0,1.0,0.01,0.1,0.01]) + R = diag([1.0,1.0,1.0,0.1,1.0,1.0,1.0]) + Q = diag([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + R = diag([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + # x,y,v,psi,psiDot,a_x,a_y # Set up track parameters l = Localization() l.create_track() l.prepare_trajectory(0.06) - d_f = 0 + d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_f_lp = 0 + a_lp = 0 # Estimation variables - (x_est, y_est) = [0]*2 + (x_est, y_est, a_x_est, a_y_est) = [0]*4 + bta = 0 + v_est = 0 + psi_est = 0 while not rospy.is_shutdown(): # make R values dependent on current measurement (robust against outliers) sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - if sq_gps_dist > 0.2: - R[0,0] = 1+10*sq_gps_dist**2 - R[1,1] = 1+10*sq_gps_dist**2 + if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: + R[0,0] = 1.0 + R[1,1] = 1.0 else: - R[0,0] = 0.1 - R[1,1] = 0.1 - + # otherwise just extrapolate measurements: + #se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) + #se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) + R[0,0] = 10.0 + R[1,1] = 10.0 + # if se.imu_updated: + # R[3,3] = 1.0 + # R[4,4] = 5.0 + # else: + # R[3,3] = 10.0 + # R[4,4] = 50.0 + # if se.vel_updated: + # R[2,2] = 0.1 + # else: + # R[2,2] = 1.0 + + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False # get measurement y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas]) # define input - u = [0,0] + #d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering + a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) + #u = [se.cmd_motor, d_f_hist.pop(0)] + u = [a_lp, d_f_lp] # build extra arguments for non-linear function args = (u, vhMdl, dt, est_mode) # apply EKF and get each state estimate - (z_EKF, P) = ekf(f_KinBkMdl_mixed_nodrift, z_EKF, P, h_KinBkMdl_mixed_nodrift, y, Q, R, args) + (z_EKF, P) = ekf(f_SensorModel, z_EKF, P, h_SensorModel, y, Q, R, args) # Read values (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate # Update track position - l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) # v = v_x - #l.find_s() - l.s = 0 - l.epsi = 0 - l.s_start = 0 + l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) + l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 # and then publish position info ros_t = rospy.get_rostime() - state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - l.coeffX.tolist(), l.coeffY.tolist(), + a_x_est, a_y_est, l.coeffX.tolist(), l.coeffY.tolist(), l.coeffTheta.tolist(), l.coeffCurvature.tolist())) # wait diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 53e7bb5d..61bc49d1 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -49,86 +49,6 @@ def f_KinBkMdl(z, u, vhMdl, dt, est_mode): return array([x_next, y_next, psi_next, v_next]) -def f_KinBkMdl_psi_drift(z, u, vhMdl, dt, est_mode): - """ - process model - input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] - output: state at next time step z[k+1] - """ - #c = array([0.5431, 1.2767, 2.1516, -2.4169]) - - # get states / inputs - x = z[0] - y = z[1] - psi = z[2] - v = z[3] - psi_drift = z[4] - - d_f = u[0] - a = u[1] - - # extract parameters - (L_a, L_b) = vhMdl - - # compute slip angle - bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) - - #print "psi_drift_model:" - #print psi - #print bta - # compute next state - x_next = x + dt*( v*cos(psi + bta) ) - y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*(v/L_b*sin(bta))# + 0*psi_drift) - v_next = v + dt*(a - 0.63*sign(v)*v**2) - psi_drift_next = psi_drift - - return array([x_next, y_next, psi_next, v_next, psi_drift_next]) - - -def f_KinBkMdl_predictive(z, u, vhMdl, dt, est_mode): - """ - process model - input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] - output: state at next time step z[k+1] - """ - #c = array([0.5431, 1.2767, 2.1516, -2.4169]) - - # get states / inputs - x = z[0] - y = z[1] - psi = z[2] - v = z[3] - - x_pred = z[4] - y_pred = z[5] - psi_pred= z[6] - v_pred = z[7] - - d_f = u[0] - a = u[1] - - # extract parameters - (L_a, L_b) = vhMdl - - # compute slip angle - bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) - - dt_pred = 0.0 - # compute next state - x_next = x + dt*( v*cos(psi + bta) ) - y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*v/L_b*sin(bta) - v_next = v + dt*(a - 0.63*sign(v)*v**2) - - x_next_pred = x_next + dt_pred*( v*cos(psi + bta) ) - y_next_pred = y_next + dt_pred*( v*sin(psi + bta) ) - psi_next_pred = psi_next + dt_pred*v/L_b*sin(bta) - v_next_pred = v_next + dt_pred*(a - 0.63*sign(v)*v**2) - - return array([x_next, y_next, psi_next, v_next, x_next_pred, y_next_pred, psi_next_pred, v_next_pred]) - - def f_KinBkMdl_mixed(z, u, vhMdl, dt, est_mode): (l_A,l_B) = vhMdl #bta = atan(l_A/(l_A+l_B)*tan(u[2])) @@ -147,25 +67,32 @@ def f_KinBkMdl_mixed(z, u, vhMdl, dt, est_mode): zNext[10] = z[10] # drift_psi return array(zNext) -def f_KinBkMdl_mixed_nodrift(z, u, vhMdl, dt, est_mode): - (l_A,l_B) = vhMdl - #bta = atan(l_A/(l_A+l_B)*tan(u[2])) - bta = math.atan2(z[3],z[2]) - #print "========================" - #print "bta = %f"%bta - #print "psi = %f"%z[6] +def f_SensorModel(z, u, vhMdl, dt, est_mode): + zNext = [0]*9 - zNext[0] = z[0] + dt*(sqrt(z[2]**2+z[3]**2)*cos(z[6] + bta)) # x - zNext[1] = z[1] + dt*(sqrt(z[2]**2+z[3]**2)*sin(z[6] + bta)) # y + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y zNext[4] = z[4] # a_x zNext[5] = z[5] # a_y - zNext[6] = z[6] + dt*(sqrt(z[2]**2+z[3]**2)/l_B*sin(bta)) # psi + zNext[6] = z[6] + dt*(z[7]) # psi zNext[7] = z[7] # psidot zNext[8] = z[8] # drift_psi return array(zNext) +def h_SensorModel(x, u, vhMdl, dt, est_mode): + """This Measurement model uses gps, imu and encoders""" + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = sqrt(x[2]**2+x[3]**2) # v + y[3] = x[6]+x[8] # psi + y[4] = x[7] # psiDot + y[5] = x[4] # a_x + y[6] = x[5] # a_y + return array(y) + def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): (l_A,l_B) = vhMdl #bta = atan(l_A/(l_A+l_B)*tan(u[2])) From a5558366d561ca365c777b928b41dbc9f2de37e7 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 11 Nov 2016 16:01:34 -0800 Subject: [PATCH 088/183] Updated low level control parameters, normalized acceleration measurement --- workspace/src/barc/src/LMPC_node.jl | 2 +- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/controller_low_level.py | 7 ++++--- .../barc/src/state_estimation_SensorModel.py | 17 ++++++++++++----- 4 files changed, 18 insertions(+), 10 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 0a133560..c30f7652 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -222,7 +222,7 @@ function main() solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) else # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev[1,:]) + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) end tt = toq() diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 78b9f198..5499eef7 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 78b9f1989884673b3f5f850637f59ccf94218168 +Subproject commit 5499eef7266f69aace5b4b050899ffb5678f6029 diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 5141c84c..d05863d7 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -50,9 +50,10 @@ def pwm_converter_callback(self, msg): if FxR == 0: self.motor_pwm = 90.0 elif FxR > 0: - #self.motor_pwm = 94.14 + 2.7678*FxR # using write() in Arduino - self.motor_pwm = 87.6 + 4.83*FxR # using writeMicroseconds() in Arduino - #self.motor_pwm = 90.0 + 2.8*FxR + #self.motor_pwm = 94.14 + 2.7678*FxR # using write() in Arduino + #self.motor_pwm = 87.6 + 4.83*FxR # using writeMicroseconds() in Arduino (old) + self.motor_pwm = max(93,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down self.motor_pwm = 93.5 + 46.73*FxR self.update_arduino() diff --git a/workspace/src/barc/src/state_estimation_SensorModel.py b/workspace/src/barc/src/state_estimation_SensorModel.py index 007cd721..e1faaaa8 100755 --- a/workspace/src/barc/src/state_estimation_SensorModel.py +++ b/workspace/src/barc/src/state_estimation_SensorModel.py @@ -44,6 +44,7 @@ class StateEst(object): a_x_meas = 0 a_y_meas = 0 imu_updated = False + att = (0,0,0) # attitude # Velocity vel_meas = 0 @@ -104,11 +105,17 @@ def imu_callback(self, data): w_z = data.angular_velocity.z a_x = data.linear_acceleration.x a_y = data.linear_acceleration.y - #a_z = data.linear_acceleration.z + a_z = data.linear_acceleration.z self.psiDot_meas = w_z - self.a_x_meas = a_x - self.a_y_meas = a_y + # The next two lines 'project' the measured linear accelerations to a horizontal plane + self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + #print "Pitch: %f"%(pitch_raw) + # print "Roll: %f"%(roll_raw) + #self.a_x_meas = a_x + #self.a_y_meas = a_y + self.att = (roll_raw,pitch_raw,yaw_raw) self.imu_updated = True def vel_est_callback(self, data): @@ -235,8 +242,8 @@ def state_estimation(): ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - a_x_est, a_y_est, l.coeffX.tolist(), l.coeffY.tolist(), - l.coeffTheta.tolist(), l.coeffCurvature.tolist())) + a_x_est, a_y_est, (0,), (0,), + (0,), l.coeffCurvature.tolist())) # wait rate.sleep() From 77fa57da46301e58ba379dd6af9d7774365dd0af Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 18 Nov 2016 15:14:55 -0800 Subject: [PATCH 089/183] Updated parameters --- eval_sim/Kalman simulation/sim_kalman_imu.jl | 51 ++- eval_sim/Kalman simulation/sim_kalman_imu2.jl | 308 +++++++++++++++ .../Kalman simulation/sim_kalman_imu_findQ.jl | 361 ++++++++++++++++++ .../Parameter estimation/find_a_mapping.jl | 42 +- .../Parameter estimation/find_d_f_delay.jl | 22 +- .../Parameter estimation/find_d_f_mapping.jl | 80 ++++ eval_sim/create_track.py | 30 +- eval_sim/eval_data.jl | 221 +++++++---- workspace/src/barc/launch/barc_sim.launch | 2 +- .../src/barc/launch/run_lmpc_remote.launch | 2 +- workspace/src/barc/msg/pos_info.msg | 2 + workspace/src/barc/src/LMPC_node.jl | 18 +- .../src/barc/src/Localization_helpers.py | 34 +- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_record.jl | 8 +- workspace/src/barc/src/barc_simulator_dyn.jl | 2 +- .../src/barc/src/controller_low_level.py | 13 +- workspace/src/barc/src/open_loop.jl | 18 +- .../state_estimation_SensorKinematicModel.py | 276 +++++++++++++ .../barc/src/state_estimation_SensorModel.py | 13 +- workspace/src/barc/src/system_models.py | 44 ++- 21 files changed, 1380 insertions(+), 169 deletions(-) create mode 100644 eval_sim/Kalman simulation/sim_kalman_imu2.jl create mode 100644 eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl create mode 100755 workspace/src/barc/src/state_estimation_SensorKinematicModel.py diff --git a/eval_sim/Kalman simulation/sim_kalman_imu.jl b/eval_sim/Kalman simulation/sim_kalman_imu.jl index 57f86b68..75b1aefd 100644 --- a/eval_sim/Kalman simulation/sim_kalman_imu.jl +++ b/eval_sim/Kalman simulation/sim_kalman_imu.jl @@ -12,12 +12,12 @@ function main(code::AbstractString) imu_meas = d_rec["imu_meas"] gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_pwm_log"] + cmd_log = d_rec["cmd_log"] #cmd_pwm_log = d_rec["cmd_pwm_log"] vel_est = d_rec["vel_est"] pos_info = d_rec["pos_info"] - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1]) + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) l_A = 0.125 @@ -33,7 +33,7 @@ function main(code::AbstractString) P = zeros(7,7) x_est = zeros(length(t),7) - P_gps_imu = zeros(9,9) + P_gps_imu = zeros(9,9,10000) x_est_gps_imu = zeros(length(t),9) yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] @@ -46,6 +46,20 @@ function main(code::AbstractString) # x, y, vx, vy, ax, ay, psi, psidot, psidrift Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0]) + + Q_acc = [dt^5/20 0 dt^4/8 0 dt^3/6 0 0 0 0; + 0 dt^5/20 0 dt^4/8 0 dt^3/6 0 0 0; + dt^4/8 0 dt^3/3 0 dt^2/2 0 0 0 0; + 0 dt^4/8 0 dt^3/3 0 dt^2/2 0 0 0; + dt^3/6 0 dt^2/2 0 dt 0 0 0 0; + 0 dt^3/6 0 dt^2/2 0 dt 0 0 0; + 0 0 0 0 0 0 dt/10 0 0; + 0 0 0 0 0 0 0 dt 0; + 0 0 0 0 0 0 0 0 dt/100]*20 + + #Q_gps_imu = Q_acc # x, y, v, psi, psidot, ax, ay for i=2:length(t) @@ -57,11 +71,14 @@ function main(code::AbstractString) att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' + #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] a_x = acc_f[1] a_y = acc_f[2] + #y_yawdot = rot_f[3] #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] @@ -69,22 +86,22 @@ function main(code::AbstractString) y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) - u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + u[i,:] = cmd_log.z[t[i]-0.2.>(cmd_log.t),:][end,:] # Adapt R-value of GPS according to distance to last point: gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - if gps_dist[i] > 0.3 + if gps_dist[i] < 0.8 R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 else - R_gps_imu[1,1] = 1 - R_gps_imu[2,2] = 1 + R_gps_imu[1,1] = 10 + R_gps_imu[2,2] = 10 end args = (u[i,:],dt,l_A,l_B) # Calculate new estimate - (x_est_gps_imu[i,:], P_gps_imu) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + (x_est_gps_imu[i,:], P_gps_imu[:,:,i]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu[:,:,i-1],h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) end #figure(5) @@ -119,6 +136,24 @@ function main(code::AbstractString) grid("on") legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + figure(5) + plot(x_est_gps_imu[:,1],x_est_gps_imu[:,2]) + grid("on") + title("x-y-view") + for i=1:10:size(pos_info.t,1) + #d_f = cmd_log.z[pos_info.t[i].>(cmd_log.t-0.2),2][end] + #bta = atan(l_A/(l_A+l_B)*tan(d_f)) + dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-+") + end + for i=1:10:size(t,1) + bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) + dir = [cos(x_est_gps_imu[i,7]+bta) sin(x_est_gps_imu[i,7]+bta)] + lin = [x_est_gps_imu[i,1:2]; x_est_gps_imu[i,1:2] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-*") + end + # ax1=subplot(211) # plot(t,y,"-x",t,x_est,"-*") # grid("on") diff --git a/eval_sim/Kalman simulation/sim_kalman_imu2.jl b/eval_sim/Kalman simulation/sim_kalman_imu2.jl new file mode 100644 index 00000000..97cf746e --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman_imu2.jl @@ -0,0 +1,308 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + + +function main(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + #cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/50 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,11) + q = zeros(sz,2) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(14,14) + x_est_gps_imu = zeros(length(t),14) + + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + y_gps_imu[1,4] = 0 + + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) + R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + # x, y, v, psi, psidot, ax, ay + + for i=2:length(t) + # Collect measurements and inputs for this iteration + # Interpolate GPS-measurements: + x_p = gps_meas.z[(t[i].>gps_meas.t) & (t[i]-1.gps_meas.t) & (t[i]-1.gps_meas.t) & (t[i]-1. 1 + c_x = [t_p.^2 t_p ones(sl,1)]\x_p + c_y = [t_p.^2 t_p ones(sl,1)]\y_p + x_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_x + y_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_y + else + x_int = 0 + y_int = 0 + end + + #println("c_x = $c_x, t = $(t[i]-t0), x_int = $x_int") + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] + y_gps = [x_int y_int] + #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + + att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] + acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] + acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' + #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' + #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = acc_f[1] + a_y = acc_f[2] + #y_yawdot = rot_f[3] + + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est] + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10]) + + u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] + u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,1][end] + + # Adapt R-value of GPS according to distance to last point: + # gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + # if gps_dist[i] < 0.8 + # R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 + # R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 + # R_gps_imu[8,8] = 1#+100*gps_dist[i]^2 + # R_gps_imu[9,9] = 1#+100*gps_dist[i]^2 + # else + # R_gps_imu[1,1] = 10 + # R_gps_imu[2,2] = 10 + # R_gps_imu[8,8] = 10 + # R_gps_imu[9,9] = 10 + # end + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + #figure(5) + #plot(t-t0,y_gps_imu) + #grid("on") + + println("Yaw0 = $yaw0") + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") + plot(t-t0,x_est_gps_imu[:,10:11],"-x") + #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") + grid("on") + legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) + title("Comparison x,y estimate and measurement") + + figure(2) + plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) + plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") + grid("on") + legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") + legend(["v_est","v_x_est","v_y_est","v_meas"]) + grid("on") + title("Velocity estimate and measurement") + + figure(4) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) + #plot(pos_info.t-t0,pos_info.z[:,17:18]) + grid("on") + legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + + # PLOT DIRECTIONS + figure(5) + plot(x_est_gps_imu[:,10],x_est_gps_imu[:,11]) + grid("on") + title("x-y-view") + for i=1:10:size(pos_info.t,1) + dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-+") + end + for i=1:10:size(t,1) + bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) + dir = [cos(x_est_gps_imu[i,12]+bta) sin(x_est_gps_imu[i,12]+bta)] # 7 = imu-yaw, 12 = model-yaw + lin = [x_est_gps_imu[i,10:11]; x_est_gps_imu[i,10:11] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-*") + end + axis("equal") + + figure(6) + plot(t-t0,q) + grid("on") + title("Errors") + nothing +end + +function h_gps_imu(x,args) + y = zeros(11) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[9] # psi + y[5] = x[8] # psiDot + y[6] = x[5] # a_x + y[7] = x[6] # a_y + y[8] = x[10] # x + y[9] = x[11] # y + y[10] = x[12]+x[14] # psi + y[11] = x[13] # v + return y +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + q_GPS = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2] + q_GPS2 = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11] + if q_GPS[1] > 0.005 + R[1,1] = 100 + R[2,2] = 100 + else + R[1,1] = 1 + R[2,2] = 1 + end + if q_GPS2[1] > 0.005 + R[8,8] = 100 + R[9,9] = 100 + else + R[8,8] = 1 + R[9,9] = 1 + end + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1, q_GPS[1], q_GPS2[1] +end + +function simModel_gps_imu(z,args) + zNext = copy(z) + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x + #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y + zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x + zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + zNext[7] = z[7] + dt*z[8] # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_psi + zNext[10] = z[10] + dt*(z[13]*cos(z[12] + bta)) # x + zNext[11] = z[11] + dt*(z[13]*sin(z[12] + bta)) # y + zNext[12] = z[12] + dt*(z[13]/l_B*sin(bta)) # psi + zNext[13] = z[13] + dt*(u[1] - 0.5*z[13]) # v + zNext[14] = z[14] # psi_drift_2 + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end diff --git a/eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl b/eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl new file mode 100644 index 00000000..b7c32e74 --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl @@ -0,0 +1,361 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + + +function main(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + #cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/50 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,7) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(9,9,10000) + A_gps_imu = zeros(9,9,10000) + x_est_gps_imu = zeros(length(t),9) + + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + y_gps_imu[1,4] = 0 + + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0]) + + #Q_gps_imu = Q_acc + # x, y, v, psi, psidot, ax, ay + + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] + #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + + att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] + acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] + acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' + #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' + #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = acc_f[1] + a_y = acc_f[2] + #y_yawdot = rot_f[3] + + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + + u[i,:] = cmd_log.z[t[i]-0.2.>(cmd_log.t),:][end,:] + + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + if gps_dist[i] < 0.8 + R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 + R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 + else + R_gps_imu[1,1] = 10 + R_gps_imu[2,2] = 10 + end + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est_gps_imu[i,:], P_gps_imu[:,:,i], A_gps_imu[:,:,i]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu[:,:,i-1],h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + Q_max = zeros(9,9) + for i=1:length(t)-1 + Q_max = Q_max + x_est_gps_imu[i+1,:]'*x_est_gps_imu[i+1,:] - + x_est_gps_imu[i+1,:]'*x_est_gps_imu[i,:]*A_gps_imu[:,:,i]' - + A_gps_imu[:,:,i]*x_est_gps_imu[i,:]'*x_est_gps_imu[i+1,:] + + A_gps_imu[:,:,i]*x_est_gps_imu[i,:]'*x_est_gps_imu[i,:]*A_gps_imu[:,:,i]' + end + Q_max /= length(t) + + #figure(5) + #plot(t-t0,y_gps_imu) + #grid("on") + + println("Yaw0 = $yaw0") + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) + plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") + grid("on") + legend(["x_est","y_est","x_meas","y_meas"]) + title("Comparison x,y estimate and measurement") + + figure(2) + plot(t-t0,x_est_gps_imu[:,[7,9]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) + plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") + grid("on") + legend(["psi_est","psi_drift_est","psi_meas"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") + legend(["v_est","v_x_est","v_y_est","v_meas"]) + grid("on") + title("Velocity estimate and measurement") + + figure(4) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) + #plot(pos_info.t-t0,pos_info.z[:,17:18]) + grid("on") + legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + + figure(5) + plot(x_est_gps_imu[:,1],x_est_gps_imu[:,2]) + grid("on") + title("x-y-view") + for i=1:10:size(pos_info.t,1) + #d_f = cmd_log.z[pos_info.t[i].>(cmd_log.t-0.2),2][end] + #bta = atan(l_A/(l_A+l_B)*tan(d_f)) + dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-+") + end + for i=1:10:size(t,1) + bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) + dir = [cos(x_est_gps_imu[i,7]+bta) sin(x_est_gps_imu[i,7]+bta)] + lin = [x_est_gps_imu[i,1:2]; x_est_gps_imu[i,1:2] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-*") + end + + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + # figure(1) + # ax=subplot(5,1,1) + # for i=1:4 + # subplot(5,1,i,sharex=ax) + # plot(t,y[:,i],t,x_est[:,i],"-*") + # grid("on") + # end + # subplot(5,1,5,sharex=ax) + # plot(cmd_pwm_log.t,cmd_pwm_log.z) + # grid("on") + + + # figure(2) + # subplot(2,1,1) + # title("Comparison raw GPS data and estimate") + # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["x_m","x_est"]) + # grid("on") + # subplot(2,1,2) + # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + # xlabel("t [s]") + # ylabel("Position [m]") + # legend(["y_m","y_est"]) + # grid("on") + + # figure(3) + # plot(t,gps_dist) + # title("GPS distances") + # grid("on") + # # figure() + # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # # grid("on") + # # title("x-y-view") + # figure(4) + # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + # title("Comparison simulation (-x) onboard-estimate (-*)") + # grid("on") + # legend(["x","y","psi","v","psi_drift"]) + + # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + # figure(5) + # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + # title("Comparison yaw") + # grid("on") + + # figure(6) + # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + # grid("on") + # title("Comparison of velocity measurement and estimate") + # legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h(x,args) + C = [eye(6) zeros(6,1)] + C[4,4] = 1 + #C[3,3] = 0 + C[3,5] = 1 + C[5,5] = 0 + C[5,6] = 1 + C[6,6] = 0 + C[6,7] = 1 + return C*x +end +function h_gps_imu(x,args) + y = zeros(7) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[9] # psi + y[5] = x[8] # psiDot + y[6] = x[5] # a_x + y[7] = x[6] # a_y + return y +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1, A +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + zNext[6] = z[6] # a_x + zNext[7] = z[7] # a_y + return zNext +end + +function simModel_gps_imu(z,args) + zNext = copy(z) + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x + #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y + zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x + zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + #zNext[7] = z[7] + dt*(z[3]/l_B*sin(bta)) # psi + zNext[7] = z[7] + dt*z[8] # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_psi + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end diff --git a/eval_sim/Parameter estimation/find_a_mapping.jl b/eval_sim/Parameter estimation/find_a_mapping.jl index 18ff44b2..418f4e18 100644 --- a/eval_sim/Parameter estimation/find_a_mapping.jl +++ b/eval_sim/Parameter estimation/find_a_mapping.jl @@ -1,6 +1,7 @@ using PyPlot using JLD +using Optim # type Measurements{T} # i::Int64 # measurement counter @@ -30,8 +31,8 @@ function main(code::AbstractString) v[i] = pos_info.z[t[i].>pos_info.t,15][end] cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] end - v_opt = v[500:end] - cmd_opt = cmd[500:end] + v_opt = v[1:end] + cmd_opt = cmd[1:end] res = optimize(c->cost(cmd_opt,v_opt,c,0),[0.001,0,0.1,0.001,0]) c = Optim.minimizer(res) @@ -41,6 +42,43 @@ function main(code::AbstractString) println("and c_2 = $c2") end +function v_over_u(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + L_b = 0.125 + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t),2) + psiDot = zeros(length(t)) + + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i,:] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,:][end,:] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + delta = atan2(psiDot*0.25,v_x) + c = ((cmd[:,1]-91)/6.5-0.5*v)./delta.^2 + c = c[abs(c).<100] + plot(c) + grid("on") + figure() + plot(v) + grid("on") + #plot(cmd[:,1],v) +end + function cost(cmd,v,c,p) cost = 0 v_s = zeros(length(v)) diff --git a/eval_sim/Parameter estimation/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl index 4ff3d49a..20c927e1 100644 --- a/eval_sim/Parameter estimation/find_d_f_delay.jl +++ b/eval_sim/Parameter estimation/find_d_f_delay.jl @@ -3,8 +3,8 @@ using PyPlot using JLD function main(code::AbstractString) - #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) L_b = 0.125 @@ -20,21 +20,33 @@ function main(code::AbstractString) t = t0+0.1:.02:t_end-0.1 v = zeros(length(t)) + #v2 = copy(v) psiDot = zeros(length(t)) - + cmd = zeros(length(t)) for i=1:length(t) - v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] - + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] end v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) v_y = L_b*psiDot + + idx = v_x.>1.1 + psiDot = psiDot[idx] + v_x = v_x[idx] + cmd = cmd[idx] delta = atan2(psiDot*0.25,v_x) + figure(1) + ax2=subplot(211) plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) grid("on") xlabel("t [s]") legend(["delta_true","delta_input"]) + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) + grid("on") figure(2) ax1=subplot(211) diff --git a/eval_sim/Parameter estimation/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl index f56cf349..61d680b5 100644 --- a/eval_sim/Parameter estimation/find_d_f_mapping.jl +++ b/eval_sim/Parameter estimation/find_d_f_mapping.jl @@ -130,4 +130,84 @@ function main_pwm(code::AbstractString) println("c1 = $(1/coeff[1])") println("c2 = $(coeff[2]/coeff[1])") +end + +function main_pwm2(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + #v2 = copy(v) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + + idx = v_x.>1.1 + psiDot = psiDot[idx] + v_x = v_x[idx] + cmd = cmd[idx] + delta = atan2(psiDot*0.25,v_x) + + c = [cmd ones(size(cmd,1))]\delta + + x = [60;120] + y = [x ones(2)]*c + + plot(cmd,delta,"*",x,y) + grid("on") + legend(["Measurements","Linear approximation"]) + xlabel("u_PWM") + ylabel("delta_F") + + figure(1) + ax2=subplot(211) + plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + xlabel("t [s]") + legend(["delta_true","delta_input"]) + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) + grid("on") + + figure(2) + ax1=subplot(211) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) + grid("on") + subplot(212,sharex=ax1) + plot(vel_est.t-t0,vel_est.z,vel_est.t-t0,mean(vel_est.z[:,2:3],2,),"--") + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br","v_mean"]) +end + +function showV_V_F_relation() + L_F = 0.125 + L_R = 0.125 + delta = -0.3:.01:0.3 + v_F = 1.0 + v = L_R/L_F*cos(delta).*sqrt(1+L_F^2/(L_F+L_R)^2*tan(delta).^2) + v2 = L_R/L_F*sqrt(1+L_F^2/(L_F+L_R)^2*tan(delta).^2) + plot(delta,v,delta,v2) + grid("on") + xlabel("delta") + ylabel("v/v_F") end \ No newline at end of file diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index 9baff604..68caee35 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -40,24 +40,22 @@ def add_curve(theta, length, angle): # theta = add_curve(theta,28,0) # GOGGLE TRACK -theta = add_curve(theta,30,0) -theta = add_curve(theta,40,-pi/2) -#theta = add_curve(theta,10,0) -theta = add_curve(theta,40,-pi/2) -theta = add_curve(theta,20,-pi/6) -theta = add_curve(theta,30,pi/3) -theta = add_curve(theta,20,-pi/6) -theta = add_curve(theta,40,-pi/2) -#theta = add_curve(theta,10,0) -theta = add_curve(theta,40,-pi/2) -theta = add_curve(theta,35,0) +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,20,-pi/6) +# theta = add_curve(theta,30,pi/3) +# theta = add_curve(theta,20,-pi/6) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,35,0) # SIMPLE TRACK -# theta = add_curve(theta,50,0) -# theta = add_curve(theta,100,-pi) -# theta = add_curve(theta,100,0) -# theta = add_curve(theta,100,-pi) -# theta = add_curve(theta,49,0) +theta = add_curve(theta,10,0) +theta = add_curve(theta,80,-pi) +theta = add_curve(theta,20,0) +theta = add_curve(theta,80,-pi) +theta = add_curve(theta,9,0) for i in range(0,size(theta)): x = hstack((x, x[-1] + cos(theta[i])*ds)) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 88ab2cd9..d259df3b 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -118,13 +118,14 @@ function eval_run(code::AbstractString) axis("equal") legend(["GPS meas","estimate"]) - # figure() - # plot(imu_meas.t-t0,imu_meas.z[:,7:9]) - # grid("on") - # title("Measured accelerations") + figure() + plot(imu_meas.t-t0,imu_meas.z[:,7:9]) + plot(pos_info.t-t0,pos_info.z[:,19:20]) + grid("on") + title("Measured accelerations") figure() - plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x") + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x",gps_meas.t_msg-t0,gps_meas.z,"--x") grid(1) title("GPS comparison") xlabel("t [s]") @@ -144,10 +145,18 @@ function eval_run(code::AbstractString) figure() title("Comparison of psi") - plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*",pos_info.t-t0,pos_info.z[:,16],"-*") - legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift"]) + plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*",pos_info.t-t0,pos_info.z[:,16],"-*",pos_info.t-t0,pos_info.z[:,14]) + legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift","psi_raw"]) grid() + figure() + plot(pos_info.t-t0,pos_info.z[:,2:3]) + legend(["e_y","e_psi"]) + title("Deviations from reference") + grid("on") + xlabel("t [s]") + ylabel("eY [m], ePsi [rad]") + # figure() # title("Raw IMU orientation data") # plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6]) @@ -256,12 +265,6 @@ function eval_LMPC(code::AbstractString) t0 = t[1] - figure(1) - plot(t-t0,step_diff) - grid("on") - title("One-step-errors") - legend(["xDot","yDot","psiDot","ePsi","eY"]) - figure(2) ax1=subplot(311) plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*") @@ -285,25 +288,6 @@ function eval_LMPC(code::AbstractString) grid("on") legend(["v_x","v_y","psiDot","ePsi","eY","s"]) - figure(4) - title("Open loop predictions") - #plot(t,state[:,[1,4,5]]) - for i=1:1:size(t,1)-10 - if sol_z[1,5,i]==0 - plot(t[i:i+10]-t0,sol_z[:,2:4,i]) - else - plot(t[i:i+10]-t0,sol_z[:,1:5,i]) - end - end - grid("on") - - figure() - title("Open loop inputs") - for i=1:1:size(t,1)-10 - plot(t[i:i+7]-t0,sol_u[1:8,2,i],t[i:i+9]-t0,sol_u[:,1,i]) - end - grid("on") - figure() subplot(311) title("c_Vx") @@ -319,41 +303,43 @@ function eval_LMPC(code::AbstractString) grid("on") # *********** CURVATURE ********************* - figure() - c = zeros(size(curv,1),1) - for i=1:size(curv,1) - s = state[i,6] - c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] - end - plot(state[:,1],c,"-o") - for i=1:2:size(curv,1) - s = sol_z[:,1,i] - c = zeros(size(curv,1),1) - c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - plot(s,c,"-*") - end - title("Curvature over path") - xlabel("Curvilinear abscissa [m]") - ylabel("Curvature") - grid() - - # track = create_track(0.3) # figure() - # hold(1) - # plot(x_est[:,1],x_est[:,2],"-*") - # title("Estimated position") - # plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - # axis("equal") - # grid(1) + # c = zeros(size(curv,1),1) + # for i=1:size(curv,1) + # s = state[i,6] + # c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + # end + # plot(state[:,6],c,"-o") + # for i=1:1:size(curv,1) + # if sol_z[1,5,i] == 0 + # s = sol_z[:,1,i] + # else + # s = sol_z[:,6,i] + # end + # c = zeros(size(curv,1),1) + # c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + # plot(s,c,"-*") + # end + # title("Curvature over path") + # xlabel("Curvilinear abscissa [m]") + # ylabel("Curvature") + # grid() + + track = create_track(0.3) + figure() + hold(1) + plot(x_est[:,1],x_est[:,2],"-*") + title("Estimated position") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + axis("equal") + grid(1) # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA: # CURRENT HEADING (PLOTTED BY A LINE) - # for i=1:size(x_est,1) - # #dir = [cos(x_est[i,3]) sin(x_est[i,3])] - # #dir2 = [cos(x_est[i,3] - state[i,3]) sin(x_est[i,3] - state[i,3])] - # #lin = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir] - # #lin2 = [x_est[i,1:2];x_est[i,1:2] + 0.05*dir2] - # #plot(lin[:,1],lin[:,2],"-o",lin2[:,1],lin2[:,2],"-*") - # end + for i=1:10:size(pos_info.t,1) + dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-+") + end # PREDICTED PATH # for i=1:4:size(x_est,1) @@ -414,6 +400,73 @@ function eval_LMPC(code::AbstractString) legend(["u","d_f"]) end +function eval_predictions(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + step_diff = d_lmpc["step_diff"] + + t0 = t[1] + + figure(1) + plot(t-t0,step_diff) + grid("on") + title("One-step-errors") + legend(["xDot","yDot","psiDot","ePsi","eY"]) + + figure(2) + ax1=subplot(411) + title("Open loop predictions e_y") + plot(pos_info.t-t0,pos_info.z[:,2],"o") + for i=1:5:size(t,1)-10 + if sol_z[1,5,i]==0 + plot(t[i:i+10]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+10]-t0,sol_z[:,4,i]) + end + end + grid("on") + + subplot(412,sharex=ax1) + title("Open loop predictions e_psi") + plot(pos_info.t-t0,pos_info.z[:,3],"o") + for i=1:5:size(t,1)-10 + if sol_z[1,5,i]==0 + plot(t[i:i+10]-t0,sol_z[:,3,i],"+") + else + plot(t[i:i+10]-t0,sol_z[:,5,i]) + end + end + grid("on") + + subplot(413,sharex=ax1) + title("Open loop predictions v") + plot(pos_info.t-t0,pos_info.z[:,4],"o") + for i=1:5:size(t,1)-10 + if sol_z[1,5,i]==0 + plot(t[i:i+10]-t0,sol_z[:,4,i],"+") + else + plot(t[i:i+10]-t0,sol_z[:,1,i]) + end + end + grid("on") + + subplot(414,sharex=ax1) + title("Open loop inputs") + for i=1:5:size(t,1)-10 + plot(t[i:i+7]-t0,sol_u[1:8,2,i],t[i:i+9]-t0,sol_u[:,1,i]) + end + grid("on") +end + + function eval_sysID(code::AbstractString) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" log_path_record = "$(homedir())/simulations/output-record-$(code).jld" @@ -451,6 +504,15 @@ function eval_sysID(code::AbstractString) subplot(212,sharex=ax1) plot(cmd_log.t-t0,cmd_log.z[:,1]) grid("on") + + figure(2) # longitudinal (xDot) + ax2=subplot(211) + plot(t-t0,c_Psi) + legend(["c1","c2","c3"]) + grid("on") + subplot(212,sharex=ax2) + plot(cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") end @@ -634,23 +696,22 @@ function create_track(w) # add_curve(theta,49,0) # GOGGLE TRACK - add_curve(theta,30,0) - add_curve(theta,40,-pi/2) - #add_curve(theta,10,0) - add_curve(theta,40,-pi/2) - add_curve(theta,20,-pi/6) - add_curve(theta,30,pi/3) - add_curve(theta,20,-pi/6) - add_curve(theta,40,-pi/2) - #add_curve(theta,10,0) - add_curve(theta,40,-pi/2) - add_curve(theta,35,0) + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,-pi/6) + # add_curve(theta,30,pi/3) + # add_curve(theta,20,-pi/6) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + # # SHORT SIMPLE track - # add_curve(theta,10,0) - # add_curve(theta,80,-pi) - # add_curve(theta,20,0) - # add_curve(theta,80,-pi) - # add_curve(theta,9,0) + add_curve(theta,10,0) + add_curve(theta,80,-pi) + add_curve(theta,20,0) + add_curve(theta,80,-pi) + add_curve(theta,9,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 4a141a5a..60ce3100 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -19,7 +19,7 @@ - + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 859371fa..15e73f65 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -18,7 +18,7 @@ - + diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index bad43795..4047127c 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -19,6 +19,8 @@ float64 v_raw float64 psi_drift float64 a_x float64 a_y +float64 a_x_raw +float64 a_y_raw float64[] coeffX float64[] coeffY float64[] coeffTheta diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index c30f7652..9c0a45ab 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -18,6 +18,7 @@ include("barc_lib/LMPC/MPC_models.jl") include("barc_lib/LMPC/coeffConstraintCost.jl") include("barc_lib/LMPC/solveMpcProblem.jl") include("barc_lib/LMPC/functions.jl") +include("barc_lib/simModel.jl") # This function is called whenever a new state estimate is received. # It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) @@ -109,10 +110,12 @@ function main() log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) log_step_diff = zeros(10000,5) + log_t_solv = zeros(10000) + log_sol_status = Array(Symbol,10000) # Initialize ROS node and topics init_node("mpc_traj") - loop_rate = Rate(10) + loop_rate = Rate(1/modelParams.dt) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,posInfo,mpcSol,oldTraj,coeffCurvature_update,z_est,x_est),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} @@ -123,13 +126,12 @@ function main() # buffer in current lap zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information - u_final = zeros(2) # contains last input of one lap step_diff = zeros(5) # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 17.76# 12.0#24.0 + posInfo.s_target = 12.0#17.76# 12.0#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -187,7 +189,6 @@ function main() println("previous state: $(zCurr[i-1,:])") # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj zCurr[1,:] = zCurr[i,:] # copy current state - u_final  = uCurr[i-1,:] # ... and input i = 1 lapStatus.currentIt = 1 # reset current iteration lapStatus.nextLap = false @@ -219,12 +220,13 @@ function main() tic() if lapStatus.currentLap <= n_pf z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states + #z_pf = simKinModel(z_pf,uPrev[3,:],0.1,trackCoeff.coeffCurvature,modelParams) # predict next state by one step solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) else # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) end - tt = toq() + log_t_solv[k+1] = toq() # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] @@ -232,11 +234,12 @@ function main() uPrev = circshift(uPrev,1) uPrev[1,:] = uCurr[i,:] - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $tt s") + println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $(log_t_solv[k+1]) s") # Logging # --------------------------- k = k + 1 # counter + log_sol_status[k] = mpcSol.solverStatus log_state[k,:] = zCurr[i,:] log_cmd[k+1,:] = uCurr[i,:] # the command is going to be pubished in the next iteration log_sol_u[:,:,k] = mpcSol.u @@ -271,7 +274,8 @@ function main() save(log_path,"oldTraj",oldTraj,"state",log_state[1:k,:],"t",log_t[1:k],"sol_z",log_sol_z[:,:,1:k],"sol_u",log_sol_u[:,:,1:k], "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], - "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:]) + "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:], + "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k]) println("Exiting LMPC node. Saved data to $log_path.") end diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index cef4d11e..7b8bc61d 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -143,24 +143,22 @@ def create_track(self): # theta = add_curve(theta,49,0) # GOGGLE TRACK: length = 17.76m - theta = add_curve(theta,30,0) - theta = add_curve(theta,40,-pi/2) - #theta = add_curve(theta,10,0) - theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,20,-pi/6) - theta = add_curve(theta,30,pi/3) - theta = add_curve(theta,20,-pi/6) - theta = add_curve(theta,40,-pi/2) - #theta = add_curve(theta,10,0) - theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,35,0) - - # SHORT SIMPLE RACETRACK (smooth curves): length = 24.0m - # theta = add_curve(theta,10,0) - # theta = add_curve(theta,80,-pi) - # theta = add_curve(theta,20,0) - # theta = add_curve(theta,80,-pi) - # theta = add_curve(theta,9,0) + # theta = add_curve(theta,30,0) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,20,-pi/6) + # theta = add_curve(theta,30,pi/3) + # theta = add_curve(theta,20,-pi/6) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,35,0) + + # SHORT SIMPLE RACETRACK (smooth curves): + theta = add_curve(theta,10,0) + theta = add_curve(theta,80,-pi) + theta = add_curve(theta,20,0) + theta = add_curve(theta,80,-pi) + theta = add_curve(theta,9,0) # SIMPLER RACETRACK (half circles as curves): diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 5499eef7..cfa3ae05 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 5499eef7266f69aace5b4b050899ffb5678f6029 +Subproject commit cfa3ae057682eb874641930a0998868f3398b7e8 diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index bbfd06db..0e95b222 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -34,16 +34,16 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,18)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,20)) # initiate node, set up publisher / subscriber topics init_node("barc_record") s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} - s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} + s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=10)::RobotOS.Subscriber{sensor_msgs.msg.Imu} s3 = Subscriber("ecu_pwm", ECU, ECU_PWM_callback, (cmd_pwm_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} - s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} + s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=10)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} - s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} + s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=10)::RobotOS.Subscriber{barc.msg.Vel_est} run_id = get_param("run_id") diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 6198fe8a..bb8f8e75 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -166,7 +166,7 @@ function main() end # GPS measurements - if i%4 == 0 # 25 Hz + if i%6 == 0 # 16 Hz x = round(z_current[i,1] + 0.02*randn(),2) # Indoor gps measures, rounded on cm y = round(z_current[i,2] + 0.02*randn(),2) if randn()>3 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index d05863d7..947bba5f 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -38,12 +38,11 @@ def pwm_converter_callback(self, msg): # Old servo control: # self.servo_pwm = 91.365 + 105.6*float(msg.servo) # New servo Control - if msg.servo < 0.0: # right curve - self.servo_pwm = 95.5 + 118.8*float(msg.servo) - elif msg.servo > 0.0: # left curve - self.servo_pwm = 90.8 + 78.9*float(msg.servo) - else: - self.servo_pwm = 90 + # if msg.servo < 0.0: # right curve + # self.servo_pwm = 95.5 + 118.8*float(msg.servo) + # elif msg.servo > 0.0: # left curve + # self.servo_pwm = 90.8 + 78.9*float(msg.servo) + self.servo_pwm = 90.0 + 89.0*float(msg.servo) # compute motor command FxR = float(msg.motor) @@ -58,7 +57,7 @@ def pwm_converter_callback(self, msg): self.motor_pwm = 93.5 + 46.73*FxR self.update_arduino() def neutralize(self): - self.motor_pwm = 60 # slow down first + self.motor_pwm = 40 # slow down first self.servo_pwm = 90 self.update_arduino() rospy.sleep(1) # slow down for 1 sec diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 03590bbb..48b35cee 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -25,7 +25,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,18)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,20)) # Initialize ROS node and topics init_node("mpc_traj") @@ -61,8 +61,9 @@ function main() chicane_speed = 1.0 chicane_turn = -0.3 - cmd_m = 93 - t_next = 8 + cmd_m = 85 + cmd_s = 60 + t_next = 7 mode = 1 # 1 = acc, 2 = break, 3 = break # Idea: 5 seconds acc, 3 seconds breaking. @@ -71,21 +72,22 @@ function main() t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 90.0 - cmd.servo = 80.0 + cmd.servo = 60.0 elseif t <= t_next if mode == 1 - cmd.motor = cmd_m + cmd.motor = 96 + cmd.servo = cmd_s elseif mode == 2 - cmd.motor = 80 + cmd.motor = 70 end elseif t <= 300 if mode == 1 - cmd_m += 1 mode = 2 else + cmd_s += 2 mode = 1 end - t_next = t + 6 + t_next = t + 4 # CHICANE: # elseif t <= 3 # cmd.motor = chicane_speed diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py new file mode 100755 index 00000000..52f3168a --- /dev/null +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -0,0 +1,276 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append +from numpy import ones, polyval, delete, size +from observers import ekf +from system_models import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0 + cmd_motor = 0 + cmd_t = 0 + + # IMU + yaw_prev = 0 + yaw0 = 0 # yaw at t = 0 + yaw_meas = 0 + psiDot_meas = 0 + a_x_meas = 0 + a_y_meas = 0 + imu_updated = False + att = (0,0,0) # attitude + + # Velocity + vel_meas = 0 + vel_updated = False + vel_prev = 0 + vel_count = 0 # this counts how often the same vel measurement has been received + + # GPS + x_meas = 0 + y_meas = 0 + gps_updated = False + x_hist = zeros(15) + y_hist = zeros(15) + t_gps = zeros(15) + c_X = array([0,0,0]) + c_Y = array([0,0,0]) + + # General variables + t0 = 0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + """This function is called when a new GPS signal is received.""" + # units: [rad] and [rad/s] + t_now = rospy.get_rostime().to_sec()-self.t0 + t_msg = data.header.stamp.to_sec()-self.t0 + #if abs(t_now - t_msg) > 0.1: + # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) + self.x_meas = data.x_m + self.y_meas = data.y_m + self.x_hist = append(self.x_hist, data.x_m) + self.y_hist = append(self.y_hist, data.y_m) + self.t_gps = append(self.t_gps, t_now) + #self.x_hist = delete(self.x_hist,0) + #self.y_hist = delete(self.y_hist,0) + #self.t_gps = delete(self.t_gps,0) + self.x_hist = self.x_hist[self.t_gps > t_now-1.0] + self.y_hist = self.y_hist[self.t_gps > t_now-1.0] + self.t_gps = self.t_gps[self.t_gps > t_now-1.0] + sz = size(self.t_gps, 0) + if sz > 0: + t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T + self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] + self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] + self.gps_updated = True + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 + + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + # The next two lines 'project' the measured linear accelerations to a horizontal plane + #self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + #self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + #print "Pitch: %f"%(pitch_raw) + # print "Roll: %f"%(roll_raw) + self.a_x_meas = a_x + self.a_y_meas = a_y + self.att = (roll_raw,pitch_raw,yaw_raw) + self.imu_updated = True + + def vel_est_callback(self, data): + #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est + if data.vel_est != self.vel_prev: + self.vel_meas = data.vel_est + self.vel_updated = True + self.vel_prev = data.vel_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() # set initial time + + z_EKF = zeros(14) # x, y, psi, v, psi_drift + P = eye(14) # initial dynamics coveriance matrix + + # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v + Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) + R = diag([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v + + # Set up track parameters + l = Localization() + l.create_track() + l.prepare_trajectory(0.06) + + d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_f_lp = 0 + a_lp = 0 + + t_now = 0 + + # Estimation variables + (x_est, y_est, a_x_est, a_y_est) = [0]*4 + bta = 0 + v_est = 0 + psi_est = 0 + + while not rospy.is_shutdown(): + t_now = rospy.get_rostime().to_sec()-se.t0 + # make R values dependent on current measurement (robust against outliers) + sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 + # if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: + # R[0,0] = 1.0 + # R[1,1] = 1.0 + # else: + # # otherwise just extrapolate measurements: + # #se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) + # #se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) + # R[0,0] = 10.0 + # R[1,1] = 10.0 + # if se.imu_updated: + # R[3,3] = 1.0 + # R[4,4] = 5.0 + # else: + # R[3,3] = 10.0 + # R[4,4] = 50.0 + # if se.vel_updated: + # R[2,2] = 0.1 + # else: + # R[2,2] = 1.0 + se.x_meas = polyval(se.c_X,t_now) + se.y_meas = polyval(se.c_Y,t_now) + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas]) + + # define input + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + #d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering + a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) + u = [a_lp, d_f_hist.pop(0)] + #u = [a_lp, d_f_lp] + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, 0) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args) + # Read values + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, + x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate + + # Update track position + l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) + l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, + a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, (0,), (0,), + (0,), l.coeffCurvature.tolist())) + + # wait + rate.sleep() + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/state_estimation_SensorModel.py b/workspace/src/barc/src/state_estimation_SensorModel.py index e1faaaa8..4900ec13 100755 --- a/workspace/src/barc/src/state_estimation_SensorModel.py +++ b/workspace/src/barc/src/state_estimation_SensorModel.py @@ -21,7 +21,7 @@ from std_msgs.msg import Header from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin from observers import ekf -from system_models import f_SensorModel, h_SensorModel +from system_models import f_SensorModel, h_SensorModel, f_KinBkMdl_2, h_KinBkMdl_2 from tf import transformations import math @@ -164,11 +164,11 @@ def state_estimation(): z_EKF = zeros(9) # x, y, psi, v, psi_drift P = eye(9) # initial dynamics coveriance matrix + #Q = diag([0.01,0.01,0.1,0.1,1.0,1.0,0.01,0.1,0.01]) + #R = diag([1.0,1.0,1.0,0.1,1.0,1.0,1.0]) # x, y, vx, vy, ax, ay, psi, psidot, psidrift - Q = diag([0.01,0.01,0.1,0.1,1.0,1.0,0.01,0.1,0.01]) - R = diag([1.0,1.0,1.0,0.1,1.0,1.0,1.0]) - Q = diag([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - R = diag([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + R = diag([1.0,1.0,1.0,0.1,10.0,100.0,100.0]) # x,y,v,psi,psiDot,a_x,a_y # Set up track parameters @@ -227,7 +227,6 @@ def state_estimation(): # apply EKF and get each state estimate (z_EKF, P) = ekf(f_SensorModel, z_EKF, P, h_SensorModel, y, Q, R, args) - # Read values (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate @@ -242,7 +241,7 @@ def state_estimation(): ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - a_x_est, a_y_est, (0,), (0,), + a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, (0,), (0,), (0,), l.coeffCurvature.tolist())) # wait diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 61bc49d1..3050d8ff 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -68,7 +68,8 @@ def f_KinBkMdl_mixed(z, u, vhMdl, dt, est_mode): return array(zNext) def f_SensorModel(z, u, vhMdl, dt, est_mode): - + """ This Sensor model uses *only* sensor data and does not consider car dynamics! + Essentially, it only integrates acceleration data and puts it together with GPS data. """ zNext = [0]*9 zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y @@ -82,7 +83,7 @@ def f_SensorModel(z, u, vhMdl, dt, est_mode): return array(zNext) def h_SensorModel(x, u, vhMdl, dt, est_mode): - """This Measurement model uses gps, imu and encoders""" + """ This is the measurement model to the sensor model above """ y = [0]*7 y[0] = x[0] # x y[1] = x[1] # y @@ -93,6 +94,43 @@ def h_SensorModel(x, u, vhMdl, dt, est_mode): y[6] = x[5] # a_y return array(y) +def f_SensorKinematicModel(z, u, vhMdl, dt, est_mode): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + (l_A,l_B) = vhMdl + bta = math.atan2(l_A*tan(u[1]),l_A+l_B) + zNext = [0]*14 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + zNext[8] = z[8] # drift_psi + zNext[9] = z[9] + dt*(z[12]*cos(z[11] + bta)) # x + zNext[10] = z[10] + dt*(z[12]*sin(z[11] + bta)) # y + zNext[11] = z[11] + dt*(z[12]/l_B*sin(bta)) # psi + zNext[12] = z[12] + dt*(u[0] - 0.5*z[12]) # v + zNext[13] = z[13] # drift_psi_2 + return array(zNext) + +def h_SensorKinematicModel(x, u, vhMdl, dt, est_mode): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*11 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = sqrt(x[2]**2+x[3]**2) # v + y[3] = x[6]+x[8] # psi + y[4] = x[7] # psiDot + y[5] = x[4] # a_x + y[6] = x[5] # a_y + y[7] = x[9] # x + y[8] = x[10] # y + y[9] = x[11]+x[13] # psi + y[10] = x[12] # v + return array(y) + def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): (l_A,l_B) = vhMdl #bta = atan(l_A/(l_A+l_B)*tan(u[2])) @@ -104,7 +142,7 @@ def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): zNext[0] = z[0] + dt*(z[3]*cos(z[2] + bta)) # x zNext[1] = z[1] + dt*(z[3]*sin(z[2] + bta)) # y zNext[2] = z[2] + dt*(z[3]/l_B*sin(bta)) # psi - zNext[3] = z[3] + dt*(u[0] - 1.0*z[3]) # v + zNext[3] = z[3] + dt*(u[0] - 0.5*z[3]) # v zNext[4] = z[4] # psiDot zNext[5] = z[5] # drift_psi return array(zNext) From b3c0ea96e31508f648fb99eaecd71b790b03c3e7 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 28 Nov 2016 08:25:47 -0800 Subject: [PATCH 090/183] Optimized Localization helpers --- .../src/barc/src/Localization_helpers.py | 85 +++++++------------ 1 file changed, 30 insertions(+), 55 deletions(-) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 7b8bc61d..5c457a63 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -43,12 +43,10 @@ class Localization(object): N_nodes_poly_front = 40 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 8 # order of x-y-polynomial interpolation + OrderXY = 6 # order of x-y-polynomial interpolation OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? - coeffX = zeros(9) - coeffY = zeros(9) coeffTheta = zeros(9) coeffCurvature = zeros(9) @@ -153,7 +151,7 @@ def create_track(self): # theta = add_curve(theta,40,-pi/2) # theta = add_curve(theta,35,0) - # SHORT SIMPLE RACETRACK (smooth curves): + # SHORT SIMPLE RACETRACK (smooth curves): 12.0m theta = add_curve(theta,10,0) theta = add_curve(theta,80,-pi) theta = add_curve(theta,20,0) @@ -308,71 +306,55 @@ def find_s(self): Matrix3rd[i,self.OrderThetaCurv-k] = (s_start + i*self.ds)**k # Solve system of equations to find polynomial coefficients (for x-y) - self.coeffX = linalg.lstsq(Matrix,nodes_X)[0] - self.coeffY = linalg.lstsq(Matrix,nodes_Y)[0] + coeffX = linalg.lstsq(Matrix,nodes_X)[0] + coeffY = linalg.lstsq(Matrix,nodes_Y)[0] # find angles and curvature along interpolation polynomial - b_theta_vec = zeros(self.nPoints) + #b_theta_vec = zeros(self.nPoints) b_curvature_vector = zeros(self.nPoints) + pdcx1 = polyder(coeffX,1) + pdcy1 = polyder(coeffY,1) + pdcx2 = polyder(coeffX,2) + pdcy2 = polyder(coeffY,2) for j in range(0,self.nPoints): s = s_start + j*self.ds - dX = polyval(polyder(self.coeffX,1),s) - dY = polyval(polyder(self.coeffY,1),s) - ddX = polyval(polyder(self.coeffX,2),s) - ddY = polyval(polyder(self.coeffY,2),s) - angle = arctan2(dY,dX) - if j>1: - if angle - b_theta_vec[j-1] > pi: - angle = angle - 2*pi - elif angle - b_theta_vec[j-1] < -pi: - angle = angle + 2*pi - - b_theta_vec[j] = angle + dX = polyval(pdcx1,s) + dY = polyval(pdcy1,s) + ddX = polyval(pdcx2,s) + ddY = polyval(pdcy2,s) + # angle = arctan2(dY,dX) + # if j>1: # unwrap angles + # if angle - b_theta_vec[j-1] > pi: + # angle = angle - 2*pi + # elif angle - b_theta_vec[j-1] < -pi: + # angle = angle + 2*pi + # b_theta_vec[j] = angle b_curvature_vector[j] = (dX*ddY-dY*ddX)/(dX**2+dY**2)**1.5 # this calculates the curvature values for all points in the interp. interval # these values are going to be approximated by a polynomial! - # print b_curvature_vector[10:20] - # print b_curvature_vector[10:15] # calculate coefficients for Theta and curvature - coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] + #coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] # not needed coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] - # if max(coeffCurvature) > 200: - # print "Large value, idx_min = %f"%(idx_min) - # print(nodes_X) - # print(nodes_Y) - - # Calculate s discretization = 0.001 # discretization to calculate s - + j = s_start + arange((self.N_nodes_poly_back-1)*self.ds,(self.N_nodes_poly_back+1)*self.ds,discretization) - #print "idx_min = %f"%idx_min - #print "s_idx_start = %f"%s_idx_start - #print "idx_start = %f"%idx_start - #print "idx_end = %f"%idx_end - #print "n = %f"%n - #print j - - #print("s_discretization:") - #print(j) - x_eval = polyval(self.coeffX,j) - y_eval = polyval(self.coeffY,j) + + x_eval = polyval(coeffX,j) + y_eval = polyval(coeffY,j) dist_eval = sum((self.pos*ones([size(j),2])-vstack((x_eval,y_eval)).transpose())**2,1)**0.5 idx_s_min = argmin(dist_eval) s = j[idx_s_min] # s = minimum distance to points between idx_min-1 and idx_min+1 eyabs = amin(dist_eval) # absolute distance to curve - #print dist_eval # Calculate sign of y s0 = s - discretization - XCurve0 = polyval(self.coeffX,s0) - YCurve0 = polyval(self.coeffY,s0) - XCurve = polyval(self.coeffX,s) - YCurve = polyval(self.coeffY,s) - dX = polyval(polyder(self.coeffX,1),s) - dY = polyval(polyder(self.coeffY,1),s) + XCurve0 = polyval(coeffX,s0) + YCurve0 = polyval(coeffY,s0) + dX = polyval(polyder(coeffX,1),s) + dY = polyval(polyder(coeffY,1),s) xyVectorAngle = arctan2(self.pos[1]-YCurve0, self.pos[0]-XCurve0) xyPathAngle = arctan2(dY,dX) @@ -381,17 +363,10 @@ def find_s(self): # Calculate epsi epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle epsi = (epsi+pi)%(2*pi)-pi - #print "epsi = %f"%epsi - #if abs(epsi) > pi/2: - # if epsi < pi/2: - # epsi = epsi + 2*pi - # else: - # epsi = epsi - 2*pi - #epsi = (self.psi+pi)%(2*pi)+pi-xyPathAngle + self.epsi = epsi self.ey = ey self.s = s - self.coeffTheta = coeffTheta self.coeffCurvature = coeffCurvature self.s_start = s_start From d2f7229452043be5f4d76542df05f9358d5e68e6 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 28 Nov 2016 08:28:18 -0800 Subject: [PATCH 091/183] Added perfect state information for simulation, added evaluation functions. --- eval_sim/Parameter estimation/puresysID.jl | 93 ++++++ eval_sim/eval_data.jl | 163 +++++++--- workspace/src/barc/launch/barc_sim.launch | 1 + workspace/src/barc/msg/Vel_est (copy).msg | 2 - workspace/src/barc/msg/pos_info.msg | 2 + workspace/src/barc/src/LMPC_node.jl | 91 ++++-- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_record.jl | 6 +- workspace/src/barc/src/barc_simulator_dyn.jl | 31 +- .../state_estimation_SensorKinematicModel.py | 18 +- ...ate_estimation_SensorKinematicModel_sim.py | 299 ++++++++++++++++++ workspace/src/barc/src/testCoeffConstr.jl | 55 ++++ 12 files changed, 664 insertions(+), 99 deletions(-) create mode 100644 eval_sim/Parameter estimation/puresysID.jl delete mode 100644 workspace/src/barc/msg/Vel_est (copy).msg create mode 100755 workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py create mode 100644 workspace/src/barc/src/testCoeffConstr.jl diff --git a/eval_sim/Parameter estimation/puresysID.jl b/eval_sim/Parameter estimation/puresysID.jl new file mode 100644 index 00000000..069d54f1 --- /dev/null +++ b/eval_sim/Parameter estimation/puresysID.jl @@ -0,0 +1,93 @@ + +using PyPlot +using JLD + +function main(code::AbstractString,n::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + delay_df = 0 + + #n = 2 + + #idx0 = (oldTraj.oldTraj[:,6,n].> 0) & (oldTraj.oldTraj[:,6,n].< 17.76) + #oldTraj.oldTraj = oldTraj.oldTraj[idx0,:,:] + #oldTraj.oldInput = oldTraj.oldInput[idx0,:,:] + oldTraj = d_lmpc["oldTraj"] + sz = size(oldTraj.oldTraj[oldTraj.oldTraj[:,1,n].<1000,:,n],1) + oldTraj.oldTraj[1:sz,:,n] = smooth(oldTraj.oldTraj[1:sz,:,n],5) + oldTraj.oldInput[1:sz,:,n] = smooth(oldTraj.oldInput[1:sz,:,n],5) + # plot(oldTraj.oldInput[1:sz,:,n]) + # for i=2:sz + # oldTraj.oldInput[i,:,n] = oldTraj.oldInput[i-1,:,n] + (oldTraj.oldInput[i,:,n]-oldTraj.oldInput[i-1,:,n])*0.5 + # end + # plot(oldTraj.oldInput[1:sz,:,n],"--") + #sz = 200 + idx_d = 20:5:sz-5 + idx = 20:5:sz-10 + sz1 = size(idx,1) + + y_psi = zeros(5*sz1) + A_psi = zeros(5*sz1,3) + y_vy = zeros(5*sz1) + A_vy = zeros(5*sz1,4) + y_vx = zeros(5*sz1) + A_vx = zeros(5*sz1,4) + + for i=0:4 + y_psi[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,3,n]) + A_psi[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,3,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldTraj[idx+i,2,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i-delay_df,2,n]] + end + c_psi = (A_psi'*A_psi)\A_psi'*y_psi + + for i=0:4 + y_vy[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,2,n]) + A_vy[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,2,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldTraj[idx+i,1,n].*oldTraj.oldTraj[idx+i,3,n] oldTraj.oldTraj[idx+i,3,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i-delay_df,2,n]] + end + c_vy = (A_vy'*A_vy)\A_vy'*y_vy + + for i=0:4 + y_vx[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,1,n]) + A_vx[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,2,n] oldTraj.oldTraj[idx+i,3,n] oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i,1,n]] + end + c_vx = (A_vx'*A_vx)\A_vx'*y_vx + + println("psi:") + println(c_psi) + println("vy:") + println(c_vy) + println("vx:") + println(c_vx) + + figure(1) + plot(A_psi*c_psi) + plot(y_psi) + plot(A_psi,"--") + title("Psi") + + figure(2) + plot(A_vy*c_vy) + plot(y_vy) + plot(A_vy,"--") + title("Vy") + + figure(3) + plot(A_vx*c_vx) + plot(y_vx) + plot(A_vx,"--") + title("Vx") + + nothing +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index d259df3b..df696eb7 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -76,19 +76,19 @@ function eval_sim(code::AbstractString) figure() title("Comparison of psi") - plot(imu_meas.t,imu_meas.z,"-x",z.t,z.z[:,5:6],pos_info.t,pos_info.z[:,10:11],"-*") + plot(imu_meas.t-t0,imu_meas.z,"-x",z.t-t0,z.z[:,5:6],pos_info.t-t0,pos_info.z[:,10:11],"-*") legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) grid() figure() title("Comparison of v") - plot(z.t,z.z[:,3:4],z.t,sqrt(z.z[:,3].^2+z.z[:,4].^2),pos_info.t,pos_info.z[:,8:9],"-*",pos_info.t,sqrt(pos_info.z[:,8].^2+pos_info.z[:,9].^2),"-*",vel_est.t,vel_est.z) + plot(z.t-t0,z.z[:,3:4],z.t-t0,sqrt(z.z[:,3].^2+z.z[:,4].^2),pos_info.t-t0,pos_info.z[:,8:9],"-*",pos_info.t-t0,sqrt(pos_info.z[:,8].^2+pos_info.z[:,9].^2),"-*",vel_est.t-t0,vel_est.z) legend(["real_xDot","real_yDot","real_v","est_xDot","est_yDot","est_v","v_x_meas"]) grid() figure() title("Comparison of x,y") - plot(z.t,z.z[:,1:2],pos_info.t,pos_info.z[:,6:7],"-*",gps_meas.t,gps_meas.z) + plot(z.t-t0,z.z[:,1:2],pos_info.t-t0,pos_info.z[:,6:7],"-*",gps_meas.t-t0,gps_meas.z) legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) grid() end @@ -303,27 +303,27 @@ function eval_LMPC(code::AbstractString) grid("on") # *********** CURVATURE ********************* - # figure() - # c = zeros(size(curv,1),1) - # for i=1:size(curv,1) - # s = state[i,6] - # c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] - # end - # plot(state[:,6],c,"-o") - # for i=1:1:size(curv,1) - # if sol_z[1,5,i] == 0 - # s = sol_z[:,1,i] - # else - # s = sol_z[:,6,i] - # end - # c = zeros(size(curv,1),1) - # c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - # plot(s,c,"-*") - # end - # title("Curvature over path") - # xlabel("Curvilinear abscissa [m]") - # ylabel("Curvature") - # grid() + figure() + c = zeros(size(curv,1),1) + for i=1:size(curv,1) + s = state[i,6] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + end + plot(state[:,6],c,"-o") + for i=1:1:size(curv,1) + if sol_z[1,5,i] == 0 + s = sol_z[:,1,i] + else + s = sol_z[:,6,i] + end + c = zeros(size(curv,1),1) + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + plot(s,c,"-*") + end + title("Curvature over path") + xlabel("Curvilinear abscissa [m]") + ylabel("Curvature") + grid() track = create_track(0.3) figure() @@ -415,53 +415,81 @@ function eval_predictions(code::AbstractString) t0 = t[1] + N = size(sol_z,1)-1 # number of steps (prediction horizon) + figure(1) plot(t-t0,step_diff) grid("on") title("One-step-errors") legend(["xDot","yDot","psiDot","ePsi","eY"]) + + figure(3) + plot(pos_info.t-t0,pos_info.z[:,11],"-o") + title("Open loop predictions psidot") + for i=1:1:size(t,1)-N + if sol_z[1,5,i]==NaN + #plot(t[i:i+N]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+N]-t0,sol_z[:,3,i],"-x") + end + end + grid("on") + + figure(4) + plot(pos_info.t-t0,pos_info.z[:,9],"-o") + title("Open loop predictions v_y") + for i=1:1:size(t,1)-N + if sol_z[1,5,i]==NaN + #plot(t[i:i+N]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+N]-t0,sol_z[:,2,i],"-x") + end + end + grid("on") + + figure(2) ax1=subplot(411) title("Open loop predictions e_y") - plot(pos_info.t-t0,pos_info.z[:,2],"o") - for i=1:5:size(t,1)-10 - if sol_z[1,5,i]==0 - plot(t[i:i+10]-t0,sol_z[:,2,i],"+") + plot(pos_info.t-t0,pos_info.z[:,2],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,2,i],"-+") else - plot(t[i:i+10]-t0,sol_z[:,4,i]) + plot(t[i:i+N]-t0,sol_z[:,5,i]) end end grid("on") subplot(412,sharex=ax1) title("Open loop predictions e_psi") - plot(pos_info.t-t0,pos_info.z[:,3],"o") - for i=1:5:size(t,1)-10 - if sol_z[1,5,i]==0 - plot(t[i:i+10]-t0,sol_z[:,3,i],"+") + plot(pos_info.t-t0,pos_info.z[:,3],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,3,i],"-+") else - plot(t[i:i+10]-t0,sol_z[:,5,i]) + plot(t[i:i+N]-t0,sol_z[:,4,i]) end end grid("on") subplot(413,sharex=ax1) title("Open loop predictions v") - plot(pos_info.t-t0,pos_info.z[:,4],"o") - for i=1:5:size(t,1)-10 - if sol_z[1,5,i]==0 - plot(t[i:i+10]-t0,sol_z[:,4,i],"+") + plot(pos_info.t-t0,pos_info.z[:,8],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,4,i],"-+") else - plot(t[i:i+10]-t0,sol_z[:,1,i]) + plot(t[i:i+N]-t0,sol_z[:,1,i]) end end grid("on") subplot(414,sharex=ax1) title("Open loop inputs") - for i=1:5:size(t,1)-10 - plot(t[i:i+7]-t0,sol_u[1:8,2,i],t[i:i+9]-t0,sol_u[:,1,i]) + for i=1:2:size(t,1)-N + plot(t[i:i+N-1]-t0,sol_u[1:N,2,i],t[i:i+N-1]-t0,sol_u[:,1,i]) end grid("on") end @@ -498,6 +526,7 @@ function eval_sysID(code::AbstractString) figure(1) # longitudinal (xDot) ax1=subplot(211) + title("Vx") plot(t-t0,c_Vx) legend(["c1","c2","c3"]) grid("on") @@ -507,12 +536,23 @@ function eval_sysID(code::AbstractString) figure(2) # longitudinal (xDot) ax2=subplot(211) + title("Psi") plot(t-t0,c_Psi) legend(["c1","c2","c3"]) grid("on") subplot(212,sharex=ax2) plot(cmd_log.t-t0,cmd_log.z[:,2]) grid("on") + + figure(3) # longitudinal (xDot) + ax3=subplot(211) + title("Vy") + plot(t-t0,c_Vy) + legend(["c1","c2","c3","c4"]) + grid("on") + subplot(212,sharex=ax3) + plot(cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") end @@ -542,26 +582,25 @@ function eval_LMPC_coeff(code::AbstractString,k::Int64) sol_u = d["sol_u"] coeffCost = d["coeffCost"] coeffConst = d["coeffConst"] - s_start = d["s_start"] - s = sol_z[:,1,k] + s = sol_z[:,6,k] ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] subplot(311) - plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) grid() - title("Position = $(s_start[k] + s[1]), k = $k") + title("Position = $(s[1]), k = $k") xlabel("s") ylabel("e_Y") subplot(312) - plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) grid() xlabel("s") ylabel("e_Psi") subplot(313) - plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) grid() xlabel("s") - ylabel("v") + ylabel("v_x") end function anim_LMPC(k1,k2) @@ -773,3 +812,31 @@ function simModel(z,u,dt,l_A,l_B) return zNext end + +function checkTimes(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_lmpc = load(log_path_LMPC) + + t_solv = d_lmpc["t_solv"] + sol_status = d_lmpc["sol_status"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + sol_status_int = zeros(size(t_solv,1)) + t = d_lmpc["t"] + + for i=1:size(sol_status,1) + if sol_status[i]==:Optimal + sol_status_int[i] = 1 + elseif sol_status[i]==:Infeasible + sol_status_int[i] = 2 + elseif sol_status[i]==:UserLimit + sol_status_int[i] = 3 + end + end + ax1=subplot(211) + plot(t,t_solv) + plot(t,sol_status_int,"*") + grid("on") + subplot(212,sharex=ax1) + plot(t,cmd) + grid("on") +end \ No newline at end of file diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 60ce3100..3abaef21 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -19,6 +19,7 @@ + diff --git a/workspace/src/barc/msg/Vel_est (copy).msg b/workspace/src/barc/msg/Vel_est (copy).msg deleted file mode 100644 index 1ade9b44..00000000 --- a/workspace/src/barc/msg/Vel_est (copy).msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -float32 vel_est \ No newline at end of file diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 4047127c..6ac52c21 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -21,6 +21,8 @@ float64 a_x float64 a_y float64 a_x_raw float64 a_y_raw +float64 u_a +float64 u_df float64[] coeffX float64[] coeffY float64[] coeffTheta diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 9c0a45ab..603d404b 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -14,22 +14,24 @@ using JLD # log msg include("barc_lib/classes.jl") +include("barc_lib/LMPC/functions.jl") include("barc_lib/LMPC/MPC_models.jl") include("barc_lib/LMPC/coeffConstraintCost.jl") include("barc_lib/LMPC/solveMpcProblem.jl") -include("barc_lib/LMPC/functions.jl") include("barc_lib/simModel.jl") # This function is called whenever a new state estimate is received. # It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) -function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,coeffCurvature_update::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data +function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,trackCoeff::TrackCoeff,z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data # update mpc initial condition z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer - coeffCurvature_update[:] = msg.coeffCurvature x_est[:] = [msg.x,msg.y,msg.psi,msg.v] - + trackCoeff.coeffCurvature = msg.coeffCurvature + # check if lap needs to be switched if z_est[6] % posInfo.s_target <= lapStatus.s_lapTrigger && lapStatus.switchLap + oldTraj.idx_end[lapStatus.currentLap] = oldTraj.count[lapStatus.currentLap] + oldTraj.oldCost[lapStatus.currentLap] = oldTraj.idx_end[lapStatus.currentLap] - oldTraj.idx_start[lapStatus.currentLap] lapStatus.currentLap += 1 lapStatus.nextLap = true lapStatus.switchLap = false @@ -39,26 +41,30 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: # save current state in oldTraj oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = z_est - oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = [mpcSol.a_x,mpcSol.d_f] + #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = [msg.u_a,msg.u_df] + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap]-1,:,lapStatus.currentLap]) oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap],lapStatus.currentLap] = to_sec(msg.header.stamp) oldTraj.count[lapStatus.currentLap] += 1 # if necessary: append to end of previous lap - if lapStatus.currentLap > 1 && z_est[6] < 3.0 + if lapStatus.currentLap > 1 && z_est[6] < 7.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target - oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [mpcSol.a_x,mpcSol.d_f] + #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1]-1,:,lapStatus.currentLap-1]) oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap-1],lapStatus.currentLap-1] = to_sec(msg.header.stamp) oldTraj.count[lapStatus.currentLap-1] += 1 end #if necessary: append to beginning of next lap - if z_est[6] > posInfo.s_target - 3.0 + if z_est[6] > posInfo.s_target - 7.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target - oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [mpcSol.a_x,mpcSol.d_f] + #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1]-1,:,lapStatus.currentLap+1]) oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap+1],lapStatus.currentLap+1] = to_sec(msg.header.stamp) oldTraj.count[lapStatus.currentLap+1] += 1 + oldTraj.idx_start[lapStatus.currentLap+1] = oldTraj.count[lapStatus.currentLap+1] end end @@ -85,19 +91,20 @@ function main() mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) + max_N = max(mpcParams.N,mpcParams_pF.N) # ROS-specific variables z_est = zeros(6) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) coeffX = zeros(9) # buffer for coeffX (only logging) coeffY = zeros(9) # buffer for coeffY (only logging) - cmd = ECU() # command type + cmd = ECU() # command type coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables - log_coeff_Cost = zeros(mpcCoeff.order+1,2,10000) - log_coeff_Const = zeros(mpcCoeff.order+1,2,5,10000) - log_sol_z = zeros(mpcParams.N+1,6,10000) - log_sol_u = zeros(mpcParams.N,2,10000) + log_coeff_Cost = NaN*ones(mpcCoeff.order+1,2,10000) + log_coeff_Const = NaN*ones(mpcCoeff.order+1,2,5,10000) + log_sol_z = NaN*ones(max_N+1,6,10000) + log_sol_u = NaN*ones(max_N,2,10000) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) log_state_x = zeros(10000,4) log_coeffX = zeros(10000,9) @@ -105,7 +112,7 @@ function main() log_t = zeros(10000,1) log_state = zeros(10000,6) log_cost = zeros(10000,6) - log_c_Vx = zeros(10000,3) + log_c_Vx = zeros(10000,4) log_c_Vy = zeros(10000,4) log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) @@ -118,7 +125,8 @@ function main() loop_rate = Rate(1/modelParams.dt) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,posInfo,mpcSol,oldTraj,coeffCurvature_update,z_est,x_est),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,posInfo,mpcSol,oldTraj,trackCoeff,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} + # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! run_id = get_param("run_id") println("Finished initialization.") @@ -138,6 +146,9 @@ function main() mpcSol.u = zeros(10,2) mpcSol.a_x = 0 mpcSol.d_f = 0 + + mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724] + mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765] # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) @@ -146,7 +157,7 @@ function main() lapStatus.currentLap = 3 oldTraj.count[3] = 200 coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) - oldTraj.count[3] = 1 + oldTraj.count[3] = 2 lapStatus.currentLap = 1 oldTraj.oldTraj[1:buffersize,6,1] = NaN*ones(buffersize,1) oldTraj.oldTraj[1:buffersize,6,2] = NaN*ones(buffersize,1) @@ -154,7 +165,9 @@ function main() uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - n_pf = 3 # number of first path-following laps (needs to be at least 2) + n_pf = 2 # number of first path-following laps (needs to be at least 2) + + opt_count = 0 # Start node while ! is_shutdown() @@ -163,19 +176,19 @@ function main() # This is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received # This guarantees a constant publishing frequency of 10 Hz # (The state can be predicted by 0.1s) - cmd.header.stamp = get_rostime() - cmd.motor = convert(Float32,mpcSol.a_x) - cmd.servo = convert(Float32,mpcSol.d_f) - publish(pub, cmd) + # cmd.header.stamp = get_rostime() + # cmd.motor = convert(Float32,mpcSol.a_x) + # cmd.servo = convert(Float32,mpcSol.d_f) + # publish(pub, cmd) # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information posInfo.s = zCurr[i,6] # update position info - trackCoeff.coeffCurvature = copy(coeffCurvature_update) + #trackCoeff.coeffCurvature = copy(coeffCurvature_update) # ============================= Pre-Logging (before solving) ================================ log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) - if size(mpcSol.z,2) <= 4 # find 1-step-error + if size(mpcSol.z,2) == 4 # find 1-step-error step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]).^2 else step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 @@ -192,6 +205,7 @@ function main() i = 1 lapStatus.currentIt = 1 # reset current iteration lapStatus.nextLap = false + # Set warm start for new solution (because s shifted by s_target) if lapStatus.currentLap <= n_pf setvalue(mdl_pF.z_Ol[:,1],mpcSol.z[:,1]-posInfo.s_target) @@ -210,7 +224,7 @@ function main() # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) + #coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) tt = toq() println("Finished coefficients, t = $tt s") end @@ -220,7 +234,6 @@ function main() tic() if lapStatus.currentLap <= n_pf z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states - #z_pf = simKinModel(z_pf,uPrev[3,:],0.1,trackCoeff.coeffCurvature,modelParams) # predict next state by one step solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) else # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) @@ -228,6 +241,23 @@ function main() end log_t_solv[k+1] = toq() + # Send command immediately, only if it is optimal! + #if mpcSol.solverStatus == :Optimal + # opt_count = 0 + #else # otherwise use the last optimal input + #mpcSol.a_x = uPrev[1,1] + #mpcSol.d_f = uPrev[1,2] + #opt_count += 1 + if opt_count >= 5 + warn("No optimal solution for $opt_count iterations.") + end + #end + + cmd.header.stamp = get_rostime() + cmd.motor = convert(Float32,mpcSol.a_x) + cmd.servo = convert(Float32,mpcSol.d_f) + publish(pub, cmd) + # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] zCurr[i,6] = posInfo.s%posInfo.s_target # save absolute position in s (for oldTrajectory) @@ -242,7 +272,6 @@ function main() log_sol_status[k] = mpcSol.solverStatus log_state[k,:] = zCurr[i,:] log_cmd[k+1,:] = uCurr[i,:] # the command is going to be pubished in the next iteration - log_sol_u[:,:,k] = mpcSol.u log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst log_cost[k,:] = mpcSol.cost @@ -251,10 +280,12 @@ function main() log_c_Vx[k,:] = mpcCoeff.c_Vx log_c_Vy[k,:] = mpcCoeff.c_Vy log_c_Psi[k,:] = mpcCoeff.c_Psi - if lapStatus.currentLap <= n_pf - log_sol_z[:,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) + if size(mpcSol.z,2) == 4 + log_sol_z[1:mpcParams_pF.N+1,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) + log_sol_u[1:mpcParams_pF.N,:,k] = mpcSol.u else - log_sol_z[:,1:6,k] = mpcSol.z + log_sol_z[1:mpcParams.N+1,1:6,k] = mpcSol.z + log_sol_u[1:mpcParams.N,:,k] = mpcSol.u end # Count one up: diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index cfa3ae05..69ee23f7 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit cfa3ae057682eb874641930a0998868f3398b7e8 +Subproject commit 69ee23f743841a9fe26d94740256fd9e8fb916be diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 0e95b222..976121e9 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -39,11 +39,11 @@ function main() # initiate node, set up publisher / subscriber topics init_node("barc_record") s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} - s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=10)::RobotOS.Subscriber{sensor_msgs.msg.Imu} + s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} s3 = Subscriber("ecu_pwm", ECU, ECU_PWM_callback, (cmd_pwm_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} - s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=10)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} + s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=30)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} - s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=10)::RobotOS.Subscriber{barc.msg.Vel_est} + s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} run_id = get_param("run_id") diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index bb8f8e75..dbb05b9d 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -14,7 +14,7 @@ =# using RobotOS -@rosimport barc.msg: ECU, Vel_est +@rosimport barc.msg: ECU, Vel_est, pos_info @rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @@ -70,6 +70,7 @@ function main() pub_gps = Publisher("hedge_pos", hedge_pos, queue_size=1)::RobotOS.Publisher{marvelmind_nav.msg.hedge_pos} pub_imu = Publisher("imu/data", Imu, queue_size=1)::RobotOS.Publisher{sensor_msgs.msg.Imu} pub_vel = Publisher("vel_est", Vel_est, queue_size=1)::RobotOS.Publisher{barc.msg.Vel_est} + real_val = Publisher("real_val", pos_info, queue_size=1)::RobotOS.Publisher{barc.msg.pos_info} s1 = Subscriber("ecu", ECU, ECU_callback, (u_current,cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} @@ -99,13 +100,14 @@ function main() modelParams.l_A = 0.125 modelParams.l_B = 0.125 modelParams.m = 1.98 - modelParams.I_z = 0.24 + modelParams.I_z = 0.03#0.24 # using homogenous distributed mass over a cuboid println("Publishing sensor information. Simulator running.") imu_data = Imu() vel_est = Vel_est() t0 = to_sec(get_rostime()) gps_data = hedge_pos() + real_data = pos_info() z_real.t_msg[1] = t0 @@ -121,9 +123,9 @@ function main() while ! is_shutdown() t_ros = get_rostime() t = to_sec(t_ros) - if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 - u_current[2] = cmd_log.z[t.>cmd_log.t+0.0,2][end] # artificial steering input delay - end + #if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 + # u_current[2] = cmd_log.z[t.>cmd_log.t,2][end] # artificial steering input delay + #end # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:], u_current', dt, modelParams) @@ -144,12 +146,23 @@ function main() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros - imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] + randn()*0.5 - imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] + randn()*0.5 + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] + randn()*0.3*0 + imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] + randn()*0.3*0 publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end + # real values + if i%2 == 0 + real_data.psiDot = z_current[i,6] + real_data.psi = z_current[i,5] + real_data.v_x = z_current[i,3] + real_data.v_y = z_current[i,4] + real_data.x = z_current[i,1] + real_data.y = z_current[i,2] + publish(real_val,real_data) + end + # Velocity measurements dist_traveled += norm(diff(z_current[i-1:i,1:2])) if i%5 == 0 # 20 Hz @@ -169,11 +182,11 @@ function main() if i%6 == 0 # 16 Hz x = round(z_current[i,1] + 0.02*randn(),2) # Indoor gps measures, rounded on cm y = round(z_current[i,2] + 0.02*randn(),2) - if randn()>3 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) + if randn()>10 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) x += 1#randn() # add random value to x and y y -= 1#randn() #sim_gps_interrupt = 6 # also do a little interruption - elseif randn()>3 && sim_gps_interrupt < 0 + elseif randn()>10 && sim_gps_interrupt < 0 sim_gps_interrupt = 10 # simulate gps-interrupt (10 steps at 25 Hz is 0.4 seconds) end if sim_gps_interrupt < 0 diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 52f3168a..459d89d4 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -182,9 +182,13 @@ def state_estimation(): z_EKF = zeros(14) # x, y, psi, v, psi_drift P = eye(14) # initial dynamics coveriance matrix + qa = 10 + qp = 50 # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) - R = diag([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + R = diag([0.5,0.5,0.5,0.1,10.0,10.0,10.0, 0.5,0.5,0.1,0.5]) + R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 0.5,0.5,0.1,0.5]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters @@ -223,10 +227,12 @@ def state_estimation(): # else: # R[3,3] = 10.0 # R[4,4] = 50.0 - # if se.vel_updated: - # R[2,2] = 0.1 - # else: - # R[2,2] = 1.0 + if se.vel_updated: + R[2,2] = 0.1 + R[10,10] = 0.1 + else: + R[2,2] = 1.0 + R[10,10] = 0.1 se.x_meas = polyval(se.c_X,t_now) se.y_meas = polyval(se.c_Y,t_now) se.gps_updated = False @@ -263,7 +269,7 @@ def state_estimation(): ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y, l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, (0,), (0,), + a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, (0,), (0,), (0,), l.coeffCurvature.tolist())) # wait diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py new file mode 100755 index 00000000..a9e6aad4 --- /dev/null +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py @@ -0,0 +1,299 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append +from numpy import ones, polyval, delete, size +from observers import ekf +from system_models import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0 + cmd_motor = 0 + cmd_t = 0 + + # IMU + yaw_prev = 0 + yaw0 = 0 # yaw at t = 0 + yaw_meas = 0 + psiDot_meas = 0 + a_x_meas = 0 + a_y_meas = 0 + imu_updated = False + att = (0,0,0) # attitude + + # Velocity + vel_meas = 0 + vel_updated = False + vel_prev = 0 + vel_count = 0 # this counts how often the same vel measurement has been received + + # GPS + x_meas = 0 + y_meas = 0 + gps_updated = False + x_hist = zeros(15) + y_hist = zeros(15) + t_gps = zeros(15) + c_X = array([0,0,0]) + c_Y = array([0,0,0]) + + x_true = 0 + y_true = 0 + v_x_true = 0 + v_y_true = 0 + psi_true = 0 + psiDot_true = 0 + + # General variables + t0 = 0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + """This function is called when a new GPS signal is received.""" + # units: [rad] and [rad/s] + t_now = rospy.get_rostime().to_sec()-self.t0 + t_msg = data.header.stamp.to_sec()-self.t0 + #if abs(t_now - t_msg) > 0.1: + # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) + self.x_meas = data.x_m + self.y_meas = data.y_m + self.x_hist = append(self.x_hist, data.x_m) + self.y_hist = append(self.y_hist, data.y_m) + self.t_gps = append(self.t_gps, t_now) + #self.x_hist = delete(self.x_hist,0) + #self.y_hist = delete(self.y_hist,0) + #self.t_gps = delete(self.t_gps,0) + self.x_hist = self.x_hist[self.t_gps > t_now-1.0] + self.y_hist = self.y_hist[self.t_gps > t_now-1.0] + self.t_gps = self.t_gps[self.t_gps > t_now-1.0] + sz = size(self.t_gps, 0) + if sz > 0: + t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T + self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] + self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] + self.gps_updated = True + + def real_val(self, data): + self.x_true = data.x + self.y_true = data.y + self.v_x_true = data.v_x + self.v_y_true = data.v_y + self.psi_true = data.psi + self.psiDot_true = data.psiDot + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 + + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + # The next two lines 'project' the measured linear accelerations to a horizontal plane + #self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + #self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + #print "Pitch: %f"%(pitch_raw) + # print "Roll: %f"%(roll_raw) + self.a_x_meas = a_x + self.a_y_meas = a_y + self.att = (roll_raw,pitch_raw,yaw_raw) + self.imu_updated = True + + def vel_est_callback(self, data): + #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est + if data.vel_est != self.vel_prev: + self.vel_meas = data.vel_est + self.vel_updated = True + self.vel_prev = data.vel_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) + rospy.Subscriber('real_val', pos_info, se.real_val) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=50) + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() # set initial time + + z_EKF = zeros(14) # x, y, psi, v, psi_drift + P = eye(14) # initial dynamics coveriance matrix + + qa = 10 + qp = 50 + # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v + Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + R = diag([0.5,0.5,0.5,0.1,10.0,10.0,10.0, 0.5,0.5,0.1,0.5]) + R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 0.5,0.5,0.1,0.5]) + # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v + + # Set up track parameters + l = Localization() + l.create_track() + l.prepare_trajectory(0.06) + + d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_f_lp = 0 + a_lp = 0 + + t_now = 0 + + # Estimation variables + (x_est, y_est, a_x_est, a_y_est) = [0]*4 + bta = 0 + v_est = 0 + psi_est = 0 + + while not rospy.is_shutdown(): + t_now = rospy.get_rostime().to_sec()-se.t0 + # make R values dependent on current measurement (robust against outliers) + sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 + # if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: + # R[0,0] = 1.0 + # R[1,1] = 1.0 + # else: + # # otherwise just extrapolate measurements: + # #se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) + # #se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) + # R[0,0] = 10.0 + # R[1,1] = 10.0 + # if se.imu_updated: + # R[3,3] = 1.0 + # R[4,4] = 5.0 + # else: + # R[3,3] = 10.0 + # R[4,4] = 50.0 + if se.vel_updated: + R[2,2] = 0.1 + R[10,10] = 0.1 + else: + R[2,2] = 1.0 + R[10,10] = 0.1 + se.x_meas = polyval(se.c_X,t_now) + se.y_meas = polyval(se.c_Y,t_now) + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas]) + + # define input + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + #d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering + a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) + u = [a_lp, d_f_hist.pop(0)] + #u = [a_lp, d_f_lp] + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, 0) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args) + # Read values + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, + x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate + + # Update track position + #l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) + l.set_pos(se.x_true, se.y_true, se.psi_true, v_x_est, v_y_est, psi_dot_est) + l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, se.x_true, se.y_true, se.v_x_true, se.v_y_true, + se.psi_true, se.psiDot_true, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, + a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, (0,), (0,), + (0,), l.coeffCurvature.tolist())) + + # wait + rate.sleep() + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/testCoeffConstr.jl b/workspace/src/barc/src/testCoeffConstr.jl new file mode 100644 index 00000000..e20c5c2b --- /dev/null +++ b/workspace/src/barc/src/testCoeffConstr.jl @@ -0,0 +1,55 @@ +using RobotOS +@rosimport barc.msg: ECU, pos_info +@rosimport data_service.msg: TimeData +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using data_service.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/coeffConstraintCost.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") + + +code = "794b" +log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" +#log_path_record = "$(homedir())/simulations/output-record-$(code).jld" +#d_rec = load(log_path_record) +d_lmpc = load(log_path_LMPC) + +buffersize = 2000 # size of oldTraj buffers + +# Define and initialize variables +# --------------------------------------------------------------- +# General LMPC variables +posInfo = PosInfo() +mpcCoeff = MpcCoeff() +lapStatus = LapStatus(1,1,false,false,0.3) +mpcSol = MpcSol() +trackCoeff = TrackCoeff() # info about track (at current position, approximated) +modelParams = ModelParams() +mpcParams = MpcParams() +mpcParams_pF = MpcParams() # for 1st lap (path following) + +InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) + +oldTraj = d_lmpc["oldTraj"] + + +posInfo.s = 5.0 +lapStatus.currentLap = 4 +oldTraj.count[3] = + +function testperf() + for i=1:100 + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) + end +end \ No newline at end of file From 7d345833ad392c5cb0e01a0569bd80c0a4d923b3 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 28 Nov 2016 15:49:07 -0800 Subject: [PATCH 092/183] Updated Kalman filter and Kalman filter simulation. --- eval_sim/Kalman simulation/sim_kalman_imu2.jl | 98 ++++++++-------- eval_sim/Parameter estimation/puresysID.jl | 14 +-- eval_sim/eval_data.jl | 10 +- workspace/src/barc/src/LMPC_node.jl | 22 ++-- .../state_estimation_SensorKinematicModel.py | 109 ++++++++++-------- workspace/src/barc/src/system_models.py | 4 +- 6 files changed, 138 insertions(+), 119 deletions(-) diff --git a/eval_sim/Kalman simulation/sim_kalman_imu2.jl b/eval_sim/Kalman simulation/sim_kalman_imu2.jl index 97cf746e..55da6865 100644 --- a/eval_sim/Kalman simulation/sim_kalman_imu2.jl +++ b/eval_sim/Kalman simulation/sim_kalman_imu2.jl @@ -29,7 +29,7 @@ function main(code::AbstractString) y = zeros(sz,6) u = zeros(sz,2) - y_gps_imu = zeros(sz,11) + y_gps_imu = zeros(sz,13) q = zeros(sz,2) P = zeros(7,7) @@ -37,28 +37,34 @@ function main(code::AbstractString) P_gps_imu = zeros(14,14) x_est_gps_imu = zeros(length(t),14) - yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end] gps_dist = zeros(length(t)) yaw_prev = yaw0 y_gps_imu[1,4] = 0 + qa = 10 + qp = 50 + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) # x, y, vx, vy, ax, ay, psi, psidot, psidrift #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) - Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) - R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) + Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + R_gps_imu = diagm([0.5,0.5,1.0,0.1,10.0,10.0,10.0, 0.5,0.5,0.1,0.5, 10.0,10.0]) # x, y, v, psi, psidot, ax, ay for i=2:length(t) # Collect measurements and inputs for this iteration # Interpolate GPS-measurements: - x_p = gps_meas.z[(t[i].>gps_meas.t) & (t[i]-1.gps_meas.t) & (t[i]-1.gps_meas.t) & (t[i]-1.gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1. 1 c_x = [t_p.^2 t_p ones(sl,1)]\x_p c_y = [t_p.^2 t_p ones(sl,1)]\y_p @@ -69,7 +75,6 @@ function main(code::AbstractString) y_int = 0 end - #println("c_x = $c_x, t = $(t[i]-t0), x_int = $x_int") y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] y_gps = [x_int y_int] #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 @@ -81,8 +86,8 @@ function main(code::AbstractString) rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' - #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] - #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + a_y = imu_meas.z[t[i].>imu_meas.t,8][end] a_x = acc_f[1] a_y = acc_f[2] #y_yawdot = rot_f[3] @@ -90,29 +95,24 @@ function main(code::AbstractString) #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est] + u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] + u[i,2] = cmd_log.z[t[i]-0.1.>cmd_log.t,2][end] + + bta = atan(0.5*tan(u[i,2])) + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10]) - u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] - u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,1][end] - - # Adapt R-value of GPS according to distance to last point: - # gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - # if gps_dist[i] < 0.8 - # R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 - # R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 - # R_gps_imu[8,8] = 1#+100*gps_dist[i]^2 - # R_gps_imu[9,9] = 1#+100*gps_dist[i]^2 - # else - # R_gps_imu[1,1] = 10 - # R_gps_imu[2,2] = 10 - # R_gps_imu[8,8] = 10 - # R_gps_imu[9,9] = 10 - # end args = (u[i,:],dt,l_A,l_B) + if y_gps_imu[i,3] == y_gps_imu[i-1,3] # if no new velocity measurement + R_gps_imu[2,2] = 1.0 + R_gps_imu[10,10] = 1.0 + else + R_gps_imu[2,2] = 0.1 + R_gps_imu[10,10] = 0.1 + end # Calculate new estimate (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) end @@ -134,21 +134,21 @@ function main(code::AbstractString) plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") grid("on") - legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas"]) + legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas","psi_onboard","psi_drift_onboard"]) figure(3) v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") - legend(["v_est","v_x_est","v_y_est","v_meas"]) + legend(["v_est","v_x_est","v_y_est","v_meas","v_x_onboard","v_y_onboard"]) grid("on") title("Velocity estimate and measurement") figure(4) - plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) - #plot(pos_info.t-t0,pos_info.z[:,17:18]) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",t-t0,x_est_gps_imu[:,5:6]) + plot(pos_info.t-t0,pos_info.z[:,17:18]) grid("on") - legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + legend(["a_x_meas","a_y_meas","a_x_est","a_y_est","a_x_onboard","a_y_onboard"]) # PLOT DIRECTIONS figure(5) @@ -176,7 +176,7 @@ function main(code::AbstractString) end function h_gps_imu(x,args) - y = zeros(11) + y = zeros(13) y[1] = x[1] # x y[2] = x[2] # y y[3] = sqrt(x[3]^2+x[4]^2) # v @@ -188,6 +188,8 @@ function h_gps_imu(x,args) y[9] = x[11] # y y[10] = x[12]+x[14] # psi y[11] = x[13] # v + y[12] = x[3] # v_x + y[13] = x[4] # v_y return y end function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) @@ -200,20 +202,20 @@ function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) P12 = P_kp1*H' q_GPS = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2] q_GPS2 = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11] - if q_GPS[1] > 0.005 - R[1,1] = 100 - R[2,2] = 100 - else - R[1,1] = 1 - R[2,2] = 1 - end - if q_GPS2[1] > 0.005 - R[8,8] = 100 - R[9,9] = 100 - else - R[8,8] = 1 - R[9,9] = 1 - end + # if q_GPS[1] > 0.005 + # R[1,1] = 100 + # R[2,2] = 100 + # else + # R[1,1] = 1 + # R[2,2] = 1 + # end + # if q_GPS2[1] > 0.005 + # R[8,8] = 100 + # R[9,9] = 100 + # else + # R[8,8] = 1 + # R[9,9] = 1 + # end K = P12*inv(H*P12+R) mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' diff --git a/eval_sim/Parameter estimation/puresysID.jl b/eval_sim/Parameter estimation/puresysID.jl index 069d54f1..0f5a20f2 100644 --- a/eval_sim/Parameter estimation/puresysID.jl +++ b/eval_sim/Parameter estimation/puresysID.jl @@ -8,7 +8,7 @@ function main(code::AbstractString,n::Int64) #d_rec = load(log_path_record) d_lmpc = load(log_path_LMPC) - delay_df = 0 + delay_df = 5 #n = 2 @@ -63,20 +63,20 @@ function main(code::AbstractString,n::Int64) figure(1) plot(A_psi*c_psi) - plot(y_psi) - plot(A_psi,"--") + plot(y_psi,"--") + #plot(A_psi,"--") title("Psi") figure(2) plot(A_vy*c_vy) - plot(y_vy) - plot(A_vy,"--") + plot(y_vy,"--") + #plot(A_vy,"--") title("Vy") figure(3) plot(A_vx*c_vx) - plot(y_vx) - plot(A_vx,"--") + plot(y_vx,"--") + #plot(A_vx,"--") title("Vx") nothing diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index df696eb7..9185c905 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -805,10 +805,10 @@ function simModel(z,u,dt,l_A,l_B) bta = atan(l_A/(l_A+l_B)*tan(u[2])) zNext = copy(z) - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.5 * z[4]) # v return zNext end @@ -837,6 +837,6 @@ function checkTimes(code::AbstractString) plot(t,sol_status_int,"*") grid("on") subplot(212,sharex=ax1) - plot(t,cmd) + plot(t,cmd,"-x") grid("on") end \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 603d404b..981a5a6e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -47,7 +47,7 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: oldTraj.count[lapStatus.currentLap] += 1 # if necessary: append to end of previous lap - if lapStatus.currentLap > 1 && z_est[6] < 7.0 + if lapStatus.currentLap > 1 && z_est[6] < 8.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] @@ -57,7 +57,7 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: end #if necessary: append to beginning of next lap - if z_est[6] > posInfo.s_target - 7.0 + if z_est[6] > posInfo.s_target - 8.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] @@ -139,7 +139,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 12.0#17.76# 12.0#24.0 + posInfo.s_target = 12.0#17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -147,8 +147,10 @@ function main() mpcSol.a_x = 0 mpcSol.d_f = 0 - mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724] - mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765] + #mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724] + mpcCoeff.c_Psi = [-0.3747957571478858,-0.005013036784512181,5.068342163488241] + #mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765] + mpcCoeff.c_Vy = [0.002968102163011754,-0.09886540158694888,0.012234790760745129,1.099308717654053] # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) @@ -165,7 +167,7 @@ function main() uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - n_pf = 2 # number of first path-following laps (needs to be at least 2) + n_pf = 2 # number of first path-following laps (needs to be at least 2) opt_count = 0 @@ -176,10 +178,10 @@ function main() # This is done at the beginning of the lap because this makes sure that the command is published 0.1s after the state has been received # This guarantees a constant publishing frequency of 10 Hz # (The state can be predicted by 0.1s) - # cmd.header.stamp = get_rostime() + cmd.header.stamp = get_rostime() # cmd.motor = convert(Float32,mpcSol.a_x) # cmd.servo = convert(Float32,mpcSol.d_f) - # publish(pub, cmd) + publish(pub, cmd) # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information @@ -253,10 +255,10 @@ function main() end #end - cmd.header.stamp = get_rostime() + #cmd.header.stamp = get_rostime() cmd.motor = convert(Float32,mpcSol.a_x) cmd.servo = convert(Float32,mpcSol.d_f) - publish(pub, cmd) + #publish(pub, cmd) # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 459d89d4..1fad257e 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -33,29 +33,29 @@ class StateEst(object): """This class contains all variables that are read from the sensors and then passed to the Kalman filter.""" # input variables - cmd_servo = 0 - cmd_motor = 0 - cmd_t = 0 + cmd_servo = 0.0 + cmd_motor = 0.0 + cmd_t = 0.0 # IMU - yaw_prev = 0 - yaw0 = 0 # yaw at t = 0 - yaw_meas = 0 - psiDot_meas = 0 - a_x_meas = 0 - a_y_meas = 0 + yaw_prev = 0.0 + yaw0 = 0.0 # yaw at t = 0 + yaw_meas = 0.0 + psiDot_meas = 0.0 + a_x_meas = 0.0 + a_y_meas = 0.0 imu_updated = False - att = (0,0,0) # attitude + att = (0.0,0.0,0.0) # attitude # Velocity - vel_meas = 0 + vel_meas = 0.0 vel_updated = False - vel_prev = 0 - vel_count = 0 # this counts how often the same vel measurement has been received + vel_prev = 0.0 + vel_count = 0.0 # this counts how often the same vel measurement has been received # GPS - x_meas = 0 - y_meas = 0 + x_meas = 0.0 + y_meas = 0.0 gps_updated = False x_hist = zeros(15) y_hist = zeros(15) @@ -63,8 +63,12 @@ class StateEst(object): c_X = array([0,0,0]) c_Y = array([0,0,0]) + # Estimator data + x_est = 0.0 + y_est = 0.0 + # General variables - t0 = 0 # Time when the estimator was started + t0 = 0.0 # Time when the estimator was started running = False # bool if the car is driving def __init__(self): @@ -83,16 +87,18 @@ def gps_callback(self, data): # units: [rad] and [rad/s] t_now = rospy.get_rostime().to_sec()-self.t0 t_msg = data.header.stamp.to_sec()-self.t0 - #if abs(t_now - t_msg) > 0.1: + # if abs(t_now - t_msg) > 0.1: # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) self.x_meas = data.x_m self.y_meas = data.y_m - self.x_hist = append(self.x_hist, data.x_m) - self.y_hist = append(self.y_hist, data.y_m) - self.t_gps = append(self.t_gps, t_now) - #self.x_hist = delete(self.x_hist,0) - #self.y_hist = delete(self.y_hist,0) - #self.t_gps = delete(self.t_gps,0) + dist = (self.x_est-data.x_m)**2 + (self.y_est-data.y_m)**2 + if dist < 1.0: + self.x_hist = append(self.x_hist, data.x_m) + self.y_hist = append(self.y_hist, data.y_m) + self.t_gps = append(self.t_gps, t_msg) + # self.x_hist = delete(self.x_hist,0) + # self.y_hist = delete(self.y_hist,0) + # self.t_gps = delete(self.t_gps,0) self.x_hist = self.x_hist[self.t_gps > t_now-1.0] self.y_hist = self.y_hist[self.t_gps > t_now-1.0] self.t_gps = self.t_gps[self.t_gps > t_now-1.0] @@ -133,12 +139,10 @@ def imu_callback(self, data): self.psiDot_meas = w_z # The next two lines 'project' the measured linear accelerations to a horizontal plane - #self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z - #self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z - #print "Pitch: %f"%(pitch_raw) - # print "Roll: %f"%(roll_raw) - self.a_x_meas = a_x - self.a_y_meas = a_y + self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + #self.a_x_meas = a_x + #self.a_y_meas = a_y self.att = (roll_raw,pitch_raw,yaw_raw) self.imu_updated = True @@ -165,7 +169,7 @@ def state_estimation(): rospy.Subscriber('imu/data', Imu, se.imu_callback) rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) rospy.Subscriber('ecu', ECU, se.ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback, queue_size=1) state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) # get vehicle dimension parameters @@ -185,10 +189,8 @@ def state_estimation(): qa = 10 qp = 50 # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v - Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) - R = diag([0.5,0.5,0.5,0.1,10.0,10.0,10.0, 0.5,0.5,0.1,0.5]) - R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 0.5,0.5,0.1,0.5]) + R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 0.5,0.5,0.1,0.5, 10.0, 10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters @@ -208,10 +210,12 @@ def state_estimation(): v_est = 0 psi_est = 0 + est_counter = 0 + while not rospy.is_shutdown(): t_now = rospy.get_rostime().to_sec()-se.t0 # make R values dependent on current measurement (robust against outliers) - sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 + # sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 # if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: # R[0,0] = 1.0 # R[1,1] = 1.0 @@ -228,26 +232,28 @@ def state_estimation(): # R[3,3] = 10.0 # R[4,4] = 50.0 if se.vel_updated: - R[2,2] = 0.1 - R[10,10] = 0.1 + R[2, 2] = 0.1 + R[10, 10] = 0.1 else: - R[2,2] = 1.0 - R[10,10] = 0.1 - se.x_meas = polyval(se.c_X,t_now) - se.y_meas = polyval(se.c_Y,t_now) + R[2, 2] = 1.0 + R[10, 10] = 1.0 + se.x_meas = polyval(se.c_X, t_now) + se.y_meas = polyval(se.c_Y, t_now) se.gps_updated = False se.imu_updated = False se.vel_updated = False - # get measurement - y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, - se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas]) # define input d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering - #d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering - a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) - u = [a_lp, d_f_hist.pop(0)] - #u = [a_lp, d_f_lp] + d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering + a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) # low pass filter on acceleration + # u = [a_lp, d_f_hist.pop(0)] + u = [a_lp, d_f_lp] + + bta = 0.5 * d_f_lp + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) # build extra arguments for non-linear function args = (u, vhMdl, dt, 0) @@ -258,9 +264,15 @@ def state_estimation(): (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate + se.x_est = x_est_2 + se.y_est = y_est_2 + # Update track position l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) - l.find_s() + + # Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz) + if est_counter%4 == 0: + l.find_s() #l.s = 0 #l.epsi = 0 #l.s_start = 0 @@ -273,6 +285,7 @@ def state_estimation(): (0,), l.coeffCurvature.tolist())) # wait + est_counter += 1 rate.sleep() if __name__ == '__main__': diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index 3050d8ff..e4944e76 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -117,7 +117,7 @@ def f_SensorKinematicModel(z, u, vhMdl, dt, est_mode): def h_SensorKinematicModel(x, u, vhMdl, dt, est_mode): """ This is the measurement model to the kinematic<->sensor model above """ - y = [0]*11 + y = [0]*13 y[0] = x[0] # x y[1] = x[1] # y y[2] = sqrt(x[2]**2+x[3]**2) # v @@ -129,6 +129,8 @@ def h_SensorKinematicModel(x, u, vhMdl, dt, est_mode): y[8] = x[10] # y y[9] = x[11]+x[13] # psi y[10] = x[12] # v + y[11] = x[2] # v_x + y[12] = x[3] # v_y return array(y) def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): From 487a3fb8e5d40eff9710838475c1942e73b48e24 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 30 Nov 2016 12:00:22 -0800 Subject: [PATCH 093/183] Added evaluation functions (incl. animation) --- .../Parameter estimation/find_a_mapping.jl | 2 + .../Parameter estimation/find_d_f_delay.jl | 15 +- .../Parameter estimation/find_d_f_mapping.jl | 2 +- eval_sim/Parameter estimation/puresysID.jl | 7 + eval_sim/eval_data.jl | 153 ++++++++++++++++++ eval_sim/test2.mp4 | Bin 0 -> 262 bytes .../src/barc/launch/open_loop_remote.launch | 3 +- workspace/src/barc/msg/pos_info.msg | 1 + workspace/src/barc/src/LMPC_node.jl | 26 +-- .../src/barc/src/Localization_helpers.py | 28 ++-- workspace/src/barc/src/open_loop.jl | 35 ++-- .../state_estimation_SensorKinematicModel.py | 20 +-- 12 files changed, 233 insertions(+), 59 deletions(-) create mode 100644 eval_sim/test2.mp4 diff --git a/eval_sim/Parameter estimation/find_a_mapping.jl b/eval_sim/Parameter estimation/find_a_mapping.jl index 418f4e18..eb869d2f 100644 --- a/eval_sim/Parameter estimation/find_a_mapping.jl +++ b/eval_sim/Parameter estimation/find_a_mapping.jl @@ -33,6 +33,8 @@ function main(code::AbstractString) end v_opt = v[1:end] cmd_opt = cmd[1:end] + v_opt = v[cmd.>94] + cmd_opt = cmd[cmd.>94] res = optimize(c->cost(cmd_opt,v_opt,c,0),[0.001,0,0.1,0.001,0]) c = Optim.minimizer(res) diff --git a/eval_sim/Parameter estimation/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl index 20c927e1..5c118c81 100644 --- a/eval_sim/Parameter estimation/find_d_f_delay.jl +++ b/eval_sim/Parameter estimation/find_d_f_delay.jl @@ -32,10 +32,10 @@ function main(code::AbstractString) v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) v_y = L_b*psiDot - idx = v_x.>1.1 - psiDot = psiDot[idx] - v_x = v_x[idx] - cmd = cmd[idx] + # idx = v_x.>1.1 + # psiDot = psiDot[idx] + # v_x = v_x[idx] + # cmd = cmd[idx] delta = atan2(psiDot*0.25,v_x) figure(1) @@ -48,6 +48,13 @@ function main(code::AbstractString) plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) grid("on") + figure(3) + plot(t-t0,delta,cmd_pwm_log.t-t0,(cmd_pwm_log.z[:,2]-90)/89.0) + grid("on") + xlabel("t [s]") + ylabel("d_f [rad]") + legend(["delta_true","delta_input"]) + figure(2) ax1=subplot(211) plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) diff --git a/eval_sim/Parameter estimation/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl index 61d680b5..7de6122b 100644 --- a/eval_sim/Parameter estimation/find_d_f_mapping.jl +++ b/eval_sim/Parameter estimation/find_d_f_mapping.jl @@ -173,7 +173,7 @@ function main_pwm2(code::AbstractString) x = [60;120] y = [x ones(2)]*c - plot(cmd,delta,"*",x,y) + plot(cmd,delta,"+",x,y) grid("on") legend(["Measurements","Linear approximation"]) xlabel("u_PWM") diff --git a/eval_sim/Parameter estimation/puresysID.jl b/eval_sim/Parameter estimation/puresysID.jl index 0f5a20f2..3da73f24 100644 --- a/eval_sim/Parameter estimation/puresysID.jl +++ b/eval_sim/Parameter estimation/puresysID.jl @@ -4,9 +4,11 @@ using JLD function main(code::AbstractString,n::Int64) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" #d_rec = load(log_path_record) d_lmpc = load(log_path_LMPC) + d_rec = load(log_path_record) delay_df = 5 @@ -16,6 +18,8 @@ function main(code::AbstractString,n::Int64) #oldTraj.oldTraj = oldTraj.oldTraj[idx0,:,:] #oldTraj.oldInput = oldTraj.oldInput[idx0,:,:] oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] sz = size(oldTraj.oldTraj[oldTraj.oldTraj[:,1,n].<1000,:,n],1) oldTraj.oldTraj[1:sz,:,n] = smooth(oldTraj.oldTraj[1:sz,:,n],5) oldTraj.oldInput[1:sz,:,n] = smooth(oldTraj.oldInput[1:sz,:,n],5) @@ -79,6 +83,9 @@ function main(code::AbstractString,n::Int64) #plot(A_vx,"--") title("Vx") + plot(oldTraj.oldTimes[:,1],oldTraj.oldTraj[:,3,1]) + plot(pos_info.t,pos_info.z[:,11]) + plot(imu_meas.t,imu_meas.z[:,3]) nothing end diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 9185c905..2daa1c02 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1,5 +1,7 @@ using JLD using PyPlot +using PyCall +@pyimport matplotlib.animation as animation using HDF5, JLD, ProfileView # pos_info[1] = s # pos_info[2] = eY @@ -400,6 +402,92 @@ function eval_LMPC(code::AbstractString) legend(["u","d_f"]) end +function eval_predictions_kin(code::AbstractString) + # This function helps to evaluate predictions of the *kinematic* model + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + z = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cmd_lmpc = d_lmpc["cmd"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + step_diff = d_lmpc["step_diff"] + + t0 = t[1] + + sz = size(z,1) + N = size(sol_z,1)-1 # number of steps (prediction horizon) + figure(1) + plot(pos_info.t-t0,pos_info.z[:,2],"--g") + plot(t-t0,z[:,5],"-b") + for i=1:sz + plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,2,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("e_y") + + figure(2) + plot(pos_info.t-t0,pos_info.z[:,3],"--g") + plot(t-t0,z[:,4],"-b") + for i=1:sz + plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,3,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("e_psi") + + figure(3) + plot(pos_info.t-t0,(pos_info.z[:,8].^2+pos_info.z[:,9].^2).^0.5,"--g") + plot(t-t0,(z[:,1].^2+z[:,2].^2).^0.5,"-b") + for i=1:sz + plot(linspace(t[i]-t0,t[i]-t0+0.1*N,N+1),sol_z[:,4,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("v") + + figure(4) + plot(cmd_log.t-t0,cmd_log.z,"--g") + plot(t-t0,cmd_lmpc,"-b") + for i=1:sz + plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-2),N-1),sol_u[1:N-1,1,i],"--xr") + plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-3),N-2),sol_u[1:N-2,2,i],"--xr") + end + grid("on") + + # Calculate one-step-errors: + one_step_err = zeros(sz,4) + for i=1:sz-1 + one_step_err[i,:] = sol_z[2,1:4,i] - [z[i+1,6] z[i+1,5] z[i+1,4] norm(z[i+1,1:2])] + end + + # Calculate 'real' d_f + L_b = 0.125 + v_x = real(sqrt(complex((pos_info.z[:,8].^2+pos_info.z[:,9].^2)-pos_info.z[:,11].^2*L_b^2))) + delta = atan2(pos_info.z[:,11]*0.25,v_x) + + figure(5) + ax1=subplot(211) + plot(t-t0,one_step_err) + grid("on") + legend(["s","ey","epsi","v"]) + subplot(212,sharex=ax1) + plot(t-t0,cmd_lmpc) + plot(cmd_log.t_msg-t0,cmd_log.z) + plot(cmd_log.t-t0,cmd_log.z,"--") + plot(pos_info.t-t0,delta) + grid("on") + +end function eval_predictions(code::AbstractString) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" log_path_record = "$(homedir())/simulations/output-record-$(code).jld" @@ -662,7 +750,58 @@ function anim_LMPC(k1,k2) end end +function visualize_tdiff(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + pos_info = d_rec["pos_info"] + t_diff = diff(pos_info.t_msg) + t_diff = t_diff[t_diff .< 0.08] + plt[:hist](t_diff,100) + grid("on") + xlabel("t_diff") + nothing +end + +function anim_run(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + pos_info = d_rec["pos_info"] + L_a = 0.125 + w = 0.15 + alpha = atan(w/2/L_a) + l = sqrt(L_a^2+(w/2)^2) + t0 = pos_info.t[1] + #Construct Figure and Plot Data + fig = figure(figsize=(10,10)) + ax = axes(xlim = (-3,3),ylim=(-5,1)) + track = create_track(0.3) + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid("on") + car = ax[:plot]([],[],"r-+")[1] + gps = ax[:plot]([],[],"g*")[1] + h_ti = ax[:title]("abc") + function init() + car[:set_data]([],[]) + return (car,None) + end + function animate(k) + i = k + 2000 + car_c = [pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+pi-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi-alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+pi+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi+alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l] + car[:set_data]([car_c[:,1]],[car_c[:,2]]) + gps[:set_data]([pos_info.z[max(1,i-100):i,12]],[pos_info.z[max(1,i-100):i,13]]) + ax[:set_title]("t = $(pos_info.t[i]-t0)") + #title(pos_info.t[i]-t0) + return (car,gps,None) + end + t=0:30 + anim = animation.FuncAnimation(fig, animate, frames=1000, interval=50) + anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); +end # ***************************************************************** # ****** HELPER FUNCTIONS ***************************************** # ***************************************************************** @@ -839,4 +978,18 @@ function checkTimes(code::AbstractString) subplot(212,sharex=ax1) plot(t,cmd,"-x") grid("on") +end + +function checkConnectivity(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + + plot(gps_meas.t,gps_meas.z,"-x",gps_meas.t_msg,gps_meas.z,"--x",pos_info.t,pos_info.z[:,12:13],"-*",pos_info.t_msg,pos_info.z[:,12:13],"--*") + grid("on") end \ No newline at end of file diff --git a/eval_sim/test2.mp4 b/eval_sim/test2.mp4 new file mode 100644 index 0000000000000000000000000000000000000000..ac1e8315b603a3329bf45ac87e0052af6f6e05bc GIT binary patch literal 262 zcmZQzU{FXasVvAW&d+6FU}6B#Kx~v)mTZ_?U}DI?z`&7Kl$r{nb5jyafb_N8{QNQ? zos(OZkpiTV0P_nlhmnB+h!6mU0~AK%J0MhIV=(~*lS)%c5`lD7ZYr1tsZ-2I$teOc zKp;0Ivna8kAP2&Okh+;U#UKZ(t}MyV2hy@Y_k#=pTkn%tmS$?EXJVjdXkZ8c4of8W literal 0 HcmV?d00001 diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index ab15063e..f292d232 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -14,7 +14,8 @@ - + + diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 6ac52c21..52d3c3a1 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -16,6 +16,7 @@ float64 x_raw float64 y_raw float64 psi_raw float64 v_raw +float64 psiDot_raw float64 psi_drift float64 a_x float64 a_y diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 981a5a6e..f6c2ada9 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -41,8 +41,8 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: # save current state in oldTraj oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = z_est - #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = [msg.u_a,msg.u_df] - oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap]-1,:,lapStatus.currentLap]) + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = [msg.u_a,msg.u_df] + #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap]-1,:,lapStatus.currentLap]) oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap],lapStatus.currentLap] = to_sec(msg.header.stamp) oldTraj.count[lapStatus.currentLap] += 1 @@ -60,8 +60,8 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: if z_est[6] > posInfo.s_target - 8.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target - #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] - oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1]-1,:,lapStatus.currentLap+1]) + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] + #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1]-1,:,lapStatus.currentLap+1]) oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap+1],lapStatus.currentLap+1] = to_sec(msg.header.stamp) oldTraj.count[lapStatus.currentLap+1] += 1 oldTraj.idx_start[lapStatus.currentLap+1] = oldTraj.count[lapStatus.currentLap+1] @@ -72,7 +72,7 @@ end function main() println("Starting LMPC node.") - buffersize = 2000 # size of oldTraj buffers + buffersize = 3000 # size of oldTraj buffers # Define and initialize variables # --------------------------------------------------------------- @@ -139,7 +139,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 12.0#17.76#24.0 + posInfo.s_target = 17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -167,7 +167,7 @@ function main() uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - n_pf = 2 # number of first path-following laps (needs to be at least 2) + n_pf = 10 # number of first path-following laps (needs to be at least 2) opt_count = 0 @@ -181,7 +181,7 @@ function main() cmd.header.stamp = get_rostime() # cmd.motor = convert(Float32,mpcSol.a_x) # cmd.servo = convert(Float32,mpcSol.d_f) - publish(pub, cmd) + # publish(pub, cmd) # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information @@ -200,8 +200,8 @@ function main() # ======================================= Lap trigger ======================================= if lapStatus.nextLap # if we are switching to the next lap... println("Finishing one lap at iteration $i") - println("current state: $(zCurr[i,:])") - println("previous state: $(zCurr[i-1,:])") + println("current state: ", zCurr[i,:]) + println("previous state: ", zCurr[i-1,:]) # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj zCurr[1,:] = zCurr[i,:] # copy current state i = 1 @@ -218,8 +218,8 @@ function main() # ======================================= Calculate input ======================================= println("=================================== NEW ITERATION # $i ===================================") - println("Current Lap: $(lapStatus.currentLap), It: $(lapStatus.currentIt)") - println("State Nr. $i = $z_est") + println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt) + println("State Nr. ", i, " = ", z_est) println("s = $(posInfo.s)") println("s_total = $(posInfo.s%posInfo.s_target)") @@ -258,7 +258,7 @@ function main() #cmd.header.stamp = get_rostime() cmd.motor = convert(Float32,mpcSol.a_x) cmd.servo = convert(Float32,mpcSol.d_f) - #publish(pub, cmd) + publish(pub, cmd) # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 5c457a63..0f1c372d 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -141,22 +141,22 @@ def create_track(self): # theta = add_curve(theta,49,0) # GOGGLE TRACK: length = 17.76m - # theta = add_curve(theta,30,0) - # theta = add_curve(theta,40,-pi/2) - # theta = add_curve(theta,40,-pi/2) - # theta = add_curve(theta,20,-pi/6) - # theta = add_curve(theta,30,pi/3) - # theta = add_curve(theta,20,-pi/6) - # theta = add_curve(theta,40,-pi/2) - # theta = add_curve(theta,40,-pi/2) - # theta = add_curve(theta,35,0) + theta = add_curve(theta,30,0) + theta = add_curve(theta,40,-pi/2) + theta = add_curve(theta,40,-pi/2) + theta = add_curve(theta,20,-pi/6) + theta = add_curve(theta,30,pi/3) + theta = add_curve(theta,20,-pi/6) + theta = add_curve(theta,40,-pi/2) + theta = add_curve(theta,40,-pi/2) + theta = add_curve(theta,35,0) # SHORT SIMPLE RACETRACK (smooth curves): 12.0m - theta = add_curve(theta,10,0) - theta = add_curve(theta,80,-pi) - theta = add_curve(theta,20,0) - theta = add_curve(theta,80,-pi) - theta = add_curve(theta,9,0) + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,80,-pi) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi) + # theta = add_curve(theta,9,0) # SIMPLER RACETRACK (half circles as curves): diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 48b35cee..d549f04a 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -58,11 +58,11 @@ function main() vel_est_log.i = 1 pos_info_log.i = 1 - chicane_speed = 1.0 - chicane_turn = -0.3 + chicane_speed = 97#1.0 + chicane_turn = [120,60] - cmd_m = 85 - cmd_s = 60 + cmd_m = 94 + cmd_s = 80 t_next = 7 mode = 1 # 1 = acc, 2 = break, 3 = break # Idea: 5 seconds acc, 3 seconds breaking. @@ -72,10 +72,10 @@ function main() t = to_sec(get_rostime())-t0 if t <= 3 cmd.motor = 90.0 - cmd.servo = 60.0 + cmd.servo = 80.0 elseif t <= t_next if mode == 1 - cmd.motor = 96 + cmd.motor = cmd_m cmd.servo = cmd_s elseif mode == 2 cmd.motor = 70 @@ -84,38 +84,41 @@ function main() if mode == 1 mode = 2 else - cmd_s += 2 + cmd_s += 0 + cmd_m += 1 mode = 1 end t_next = t + 4 + # ************************************** # CHICANE: # elseif t <= 3 # cmd.motor = chicane_speed - # cmd.servo = -chicane_turn + # cmd.servo = chicane_turn[1] # elseif t <= 4 # cmd.motor = chicane_speed - # cmd.servo = chicane_turn + # cmd.servo = chicane_turn[2] # elseif t <= 5 # cmd.motor = chicane_speed - # cmd.servo = -chicane_turn + # cmd.servo = chicane_turn[1] # elseif t <= 6 # cmd.motor = chicane_speed - # cmd.servo = chicane_turn + # cmd.servo = chicane_turn[2] # elseif t <= 7 # cmd.motor = chicane_speed - # cmd.servo = -chicane_turn + # cmd.servo = chicane_turn[1] # elseif t <= 8 # cmd.motor = chicane_speed - # cmd.servo = chicane_turn + # cmd.servo = chicane_turn[2] # elseif t <= 9 # cmd.motor = chicane_speed - # cmd.servo = -chicane_turn + # cmd.servo = chicane_turn[1] # elseif t <= 10 # cmd.motor = chicane_speed - # cmd.servo = chicane_turn + # cmd.servo = chicane_turn[2] # elseif t <= 11 # cmd.motor = chicane_speed - # cmd.servo = -chicane_turn + # cmd.servo = chicane_turn[1] + # ************************************** # CONTINUOUS ACCELERATION: # elseif t <= 123 # cmd.motor = 0.0+(t-3)/20 diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 1fad257e..496f857a 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -99,11 +99,11 @@ def gps_callback(self, data): # self.x_hist = delete(self.x_hist,0) # self.y_hist = delete(self.y_hist,0) # self.t_gps = delete(self.t_gps,0) - self.x_hist = self.x_hist[self.t_gps > t_now-1.0] - self.y_hist = self.y_hist[self.t_gps > t_now-1.0] - self.t_gps = self.t_gps[self.t_gps > t_now-1.0] + self.x_hist = self.x_hist[self.t_gps > t_now-1.5] + self.y_hist = self.y_hist[self.t_gps > t_now-1.5] + self.t_gps = self.t_gps[self.t_gps > t_now-1.5] sz = size(self.t_gps, 0) - if sz > 0: + if sz > 4: t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] @@ -190,7 +190,7 @@ def state_estimation(): qp = 50 # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) - R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 0.5,0.5,0.1,0.5, 10.0, 10.0]) + R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters @@ -247,8 +247,8 @@ def state_estimation(): d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) # low pass filter on acceleration - # u = [a_lp, d_f_hist.pop(0)] - u = [a_lp, d_f_lp] + u = [a_lp, d_f_hist.pop(0)] + #u = [a_lp, d_f_lp] bta = 0.5 * d_f_lp # get measurement @@ -280,9 +280,9 @@ def state_estimation(): # and then publish position info ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, (0,), (0,), - (0,), l.coeffCurvature.tolist())) + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas, + psi_drift_est, a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, + (0,), (0,), (0,), l.coeffCurvature.tolist())) # wait est_counter += 1 From 6ea5b0745b301cf4a344167c83f984afbebfc268 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Thu, 1 Dec 2016 15:41:17 -0800 Subject: [PATCH 094/183] Updated parameters and wifi settings. --- env_loader_odroid.sh | 2 +- env_loader_pc.sh | 4 +- eval_sim/Kalman simulation/sim_kalman_imu2.jl | 87 +++++++++++++----- eval_sim/Parameter estimation/puresysID.jl | 11 ++- eval_sim/eval_data.jl | 28 +++--- eval_sim/test2.mp4 | Bin 262 -> 0 bytes .../src/barc/launch/run_lmpc_remote.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 10 +- workspace/src/barc/src/barc_lib | 2 +- .../state_estimation_SensorKinematicModel.py | 37 ++++---- 10 files changed, 116 insertions(+), 67 deletions(-) delete mode 100644 eval_sim/test2.mp4 diff --git a/env_loader_odroid.sh b/env_loader_odroid.sh index 541853ee..d49076e1 100755 --- a/env_loader_odroid.sh +++ b/env_loader_odroid.sh @@ -4,5 +4,5 @@ source /opt/ros/indigo/setup.bash source ~/barc/workspace/devel/setup.bash export PATH=$PATH:/home/odroid/julia export ROS_IP=192.168.100.100 -export ROS_MASTER_URI=http://192.168.100.101:11311 +export ROS_MASTER_URI=http://192.168.100.152:11311 exec "$@" \ No newline at end of file diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 8514434f..0c38405f 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -2,6 +2,6 @@ source /opt/ros/kinetic/setup.bash source /home/max/barc/workspace/devel/setup.bash export PATH=$PATH:/home/max/Downloads/julia-2e358ce975/bin -export ROS_IP=192.168.100.101 -export ROS_MASTER_URI=http://192.168.100.101:11311 +export ROS_IP=192.168.100.152 +export ROS_MASTER_URI=http://192.168.100.152:11311 exec "$@" \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_imu2.jl b/eval_sim/Kalman simulation/sim_kalman_imu2.jl index 55da6865..3f3536a1 100644 --- a/eval_sim/Kalman simulation/sim_kalman_imu2.jl +++ b/eval_sim/Kalman simulation/sim_kalman_imu2.jl @@ -43,18 +43,18 @@ function main(code::AbstractString) yaw_prev = yaw0 y_gps_imu[1,4] = 0 - qa = 10 - qp = 50 + qa = 1000 + qp = 1000 #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) - # x, y, vx, vy, ax, ay, psi, psidot, psidrift #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) - Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1]) #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) - R_gps_imu = diagm([0.5,0.5,1.0,0.1,10.0,10.0,10.0, 0.5,0.5,0.1,0.5, 10.0,10.0]) - # x, y, v, psi, psidot, ax, ay + R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + # x, y, v, psi, psidot, ax, a_y x, y, psi, v, v_x, v_y for i=2:length(t) # Collect measurements and inputs for this iteration @@ -77,7 +77,8 @@ function main(code::AbstractString) y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] y_gps = [x_int y_int] - #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_gps = pos_info.z[t[i].>pos_info.t,12:13][end,:] + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] @@ -96,7 +97,7 @@ function main(code::AbstractString) y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] - u[i,2] = cmd_log.z[t[i]-0.1.>cmd_log.t,2][end] + u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end] bta = atan(0.5*tan(u[i,2])) y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] @@ -106,13 +107,13 @@ function main(code::AbstractString) args = (u[i,:],dt,l_A,l_B) - if y_gps_imu[i,3] == y_gps_imu[i-1,3] # if no new velocity measurement - R_gps_imu[2,2] = 1.0 - R_gps_imu[10,10] = 1.0 - else - R_gps_imu[2,2] = 0.1 - R_gps_imu[10,10] = 0.1 - end + # if y_gps_imu[i,3] == y_gps_imu[i-1,3] # if no new velocity measurement + # R_gps_imu[2,2] = 1.0 + # R_gps_imu[10,10] = 1.0 + # else + # R_gps_imu[2,2] = 1.0#0.1 + # R_gps_imu[10,10] = 1.0#0.1 + # end # Calculate new estimate (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) end @@ -125,6 +126,7 @@ function main(code::AbstractString) figure(1) plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") plot(t-t0,x_est_gps_imu[:,10:11],"-x") + plot(pos_info.t-t0,pos_info.z[:,12:13],"..+") #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") grid("on") legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) @@ -145,10 +147,9 @@ function main(code::AbstractString) title("Velocity estimate and measurement") figure(4) - plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",t-t0,x_est_gps_imu[:,5:6]) - plot(pos_info.t-t0,pos_info.z[:,17:18]) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",pos_info.t-t0,pos_info.z[:,17:18],t-t0,x_est_gps_imu[:,5:6]) grid("on") - legend(["a_x_meas","a_y_meas","a_x_est","a_y_est","a_x_onboard","a_y_onboard"]) + legend(["a_x_meas","a_y_meas","a_x_onboard","a_y_onboard","a_x_est","a_y_est",]) # PLOT DIRECTIONS figure(5) @@ -168,10 +169,14 @@ function main(code::AbstractString) end axis("equal") - figure(6) - plot(t-t0,q) + # figure(6) + # plot(t-t0,q) + # grid("on") + # title("Errors") + figure(7) + plot(imu_meas.t-t0,imu_meas.z[:,3],pos_info.t-t0,pos_info.z[:,11],t-t0,x_est_gps_imu[:,8]) + legend(["psiDot_meas","psiDot_onboard","psiDot_est"]) grid("on") - title("Errors") nothing end @@ -308,3 +313,43 @@ function rotMatrix(s::Char,deg::Float64) end return A end + +function plot_velocities(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + cmd_log = d_rec["cmd_log"] + + acc = diff(vel_est.z[:,1])./diff(vel_est.t_msg) + acc = smooth(acc,5) + t0 = vel_est.t[1] + ax1=subplot(211) + plot(vel_est.t-t0,vel_est.z) + plot(vel_est.t_msg-t0,vel_est.z,"--") + legend(["vel","fl","fr","bl","br"]) + grid("on") + xlabel("t [s]") + title("Velocity") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + plot(cmd_log.t_msg-t0,cmd_log.z[:,1],"--") + plot(vel_est.t_msg[1:end-1]-t0,acc) + grid("on") + legend(["u_a","u_a_sent","acceleration"]) + title("Commands") + xlabel("t [s]") + ylabel("") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/puresysID.jl b/eval_sim/Parameter estimation/puresysID.jl index 3da73f24..c2cb3a40 100644 --- a/eval_sim/Parameter estimation/puresysID.jl +++ b/eval_sim/Parameter estimation/puresysID.jl @@ -21,8 +21,8 @@ function main(code::AbstractString,n::Int64) pos_info = d_rec["pos_info"] imu_meas = d_rec["imu_meas"] sz = size(oldTraj.oldTraj[oldTraj.oldTraj[:,1,n].<1000,:,n],1) - oldTraj.oldTraj[1:sz,:,n] = smooth(oldTraj.oldTraj[1:sz,:,n],5) - oldTraj.oldInput[1:sz,:,n] = smooth(oldTraj.oldInput[1:sz,:,n],5) + oldTraj.oldTraj[1:sz,:,n] = smooth(oldTraj.oldTraj[1:sz,:,n],20) + #oldTraj.oldInput[1:sz,:,n] = smooth(oldTraj.oldInput[1:sz,:,n],2) # plot(oldTraj.oldInput[1:sz,:,n]) # for i=2:sz # oldTraj.oldInput[i,:,n] = oldTraj.oldInput[i-1,:,n] + (oldTraj.oldInput[i,:,n]-oldTraj.oldInput[i-1,:,n])*0.5 @@ -83,9 +83,10 @@ function main(code::AbstractString,n::Int64) #plot(A_vx,"--") title("Vx") - plot(oldTraj.oldTimes[:,1],oldTraj.oldTraj[:,3,1]) - plot(pos_info.t,pos_info.z[:,11]) - plot(imu_meas.t,imu_meas.z[:,3]) + # figure(4) + # plot(oldTraj.oldTimes[:,1],oldTraj.oldTraj[:,3,1]) + # plot(pos_info.t,pos_info.z[:,11]) + # plot(imu_meas.t,imu_meas.z[:,3]) nothing end diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 2daa1c02..d5e94409 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -874,22 +874,22 @@ function create_track(w) # add_curve(theta,49,0) # GOGGLE TRACK - # add_curve(theta,30,0) - # add_curve(theta,40,-pi/2) - # add_curve(theta,40,-pi/2) - # add_curve(theta,20,-pi/6) - # add_curve(theta,30,pi/3) - # add_curve(theta,20,-pi/6) - # add_curve(theta,40,-pi/2) - # add_curve(theta,40,-pi/2) - # add_curve(theta,35,0) + add_curve(theta,30,0) + add_curve(theta,40,-pi/2) + add_curve(theta,40,-pi/2) + add_curve(theta,20,-pi/6) + add_curve(theta,30,pi/3) + add_curve(theta,20,-pi/6) + add_curve(theta,40,-pi/2) + add_curve(theta,40,-pi/2) + add_curve(theta,35,0) # # SHORT SIMPLE track - add_curve(theta,10,0) - add_curve(theta,80,-pi) - add_curve(theta,20,0) - add_curve(theta,80,-pi) - add_curve(theta,9,0) + # add_curve(theta,10,0) + # add_curve(theta,80,-pi) + # add_curve(theta,20,0) + # add_curve(theta,80,-pi) + # add_curve(theta,9,0) for i=1:length(theta) push!(x, x[end] + cos(theta[i])*ds) diff --git a/eval_sim/test2.mp4 b/eval_sim/test2.mp4 deleted file mode 100644 index ac1e8315b603a3329bf45ac87e0052af6f6e05bc..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 262 zcmZQzU{FXasVvAW&d+6FU}6B#Kx~v)mTZ_?U}DI?z`&7Kl$r{nb5jyafb_N8{QNQ? zos(OZkpiTV0P_nlhmnB+h!6mU0~AK%J0MhIV=(~*lS)%c5`lD7ZYr1tsZ-2I$teOc zKp;0Ivna8kAP2&Okh+;U#UKZ(t}MyV2hy@Y_k#=pTkn%tmS$?EXJVjdXkZ8c4of8W diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 15e73f65..3d0e2ea9 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -8,7 +8,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index f6c2ada9..ad042a89 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -50,8 +50,8 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: if lapStatus.currentLap > 1 && z_est[6] < 8.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target - #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] - oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1]-1,:,lapStatus.currentLap-1]) + oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] + #oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] += 0.5*([msg.u_a msg.u_df]-oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1]-1,:,lapStatus.currentLap-1]) oldTraj.oldTimes[oldTraj.count[lapStatus.currentLap-1],lapStatus.currentLap-1] = to_sec(msg.header.stamp) oldTraj.count[lapStatus.currentLap-1] += 1 end @@ -159,7 +159,7 @@ function main() lapStatus.currentLap = 3 oldTraj.count[3] = 200 coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) - oldTraj.count[3] = 2 + oldTraj.count[3] = 1 lapStatus.currentLap = 1 oldTraj.oldTraj[1:buffersize,6,1] = NaN*ones(buffersize,1) oldTraj.oldTraj[1:buffersize,6,2] = NaN*ones(buffersize,1) @@ -181,7 +181,7 @@ function main() cmd.header.stamp = get_rostime() # cmd.motor = convert(Float32,mpcSol.a_x) # cmd.servo = convert(Float32,mpcSol.d_f) - # publish(pub, cmd) + publish(pub, cmd) # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter zCurr[i,:] = copy(z_est) # update state information @@ -258,7 +258,7 @@ function main() #cmd.header.stamp = get_rostime() cmd.motor = convert(Float32,mpcSol.a_x) cmd.servo = convert(Float32,mpcSol.d_f) - publish(pub, cmd) + #publish(pub, cmd) # Write current input information uCurr[i,:] = [mpcSol.a_x mpcSol.d_f] diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 69ee23f7..1187f975 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 69ee23f743841a9fe26d94740256fd9e8fb916be +Subproject commit 1187f97584a49ecbe3b1ed7741bbaecd9ac7a1bf diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 496f857a..450fab91 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -99,9 +99,9 @@ def gps_callback(self, data): # self.x_hist = delete(self.x_hist,0) # self.y_hist = delete(self.y_hist,0) # self.t_gps = delete(self.t_gps,0) - self.x_hist = self.x_hist[self.t_gps > t_now-1.5] - self.y_hist = self.y_hist[self.t_gps > t_now-1.5] - self.t_gps = self.t_gps[self.t_gps > t_now-1.5] + self.x_hist = self.x_hist[self.t_gps > t_now-1.0] + self.y_hist = self.y_hist[self.t_gps > t_now-1.0] + self.t_gps = self.t_gps[self.t_gps > t_now-1.0] sz = size(self.t_gps, 0) if sz > 4: t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T @@ -186,11 +186,14 @@ def state_estimation(): z_EKF = zeros(14) # x, y, psi, v, psi_drift P = eye(14) # initial dynamics coveriance matrix - qa = 10 - qp = 50 + qa = 1000 + qp = 1000 # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v - Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) - R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) + #Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) + + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.01,0.01,1.0,1.0,0.1]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters @@ -231,12 +234,12 @@ def state_estimation(): # else: # R[3,3] = 10.0 # R[4,4] = 50.0 - if se.vel_updated: - R[2, 2] = 0.1 - R[10, 10] = 0.1 - else: - R[2, 2] = 1.0 - R[10, 10] = 1.0 + # if se.vel_updated: + # R[2, 2] = 1.0 + # R[10, 10] = 1.0 + # else: + # R[2, 2] = 1.0 + # R[10, 10] = 1.0 se.x_meas = polyval(se.c_X, t_now) se.y_meas = polyval(se.c_Y, t_now) se.gps_updated = False @@ -246,11 +249,11 @@ def state_estimation(): # define input d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering - a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) # low pass filter on acceleration - u = [a_lp, d_f_hist.pop(0)] - #u = [a_lp, d_f_lp] + a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration + #u = [a_lp, d_f_hist.pop(0)] + u = [se.cmd_motor, d_f_hist.pop(0)] - bta = 0.5 * d_f_lp + bta = 0.5 * u[1] # get measurement y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) From df7a79d8109dd7376cb5b7091e001ac983142d48 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Fri, 2 Dec 2016 10:22:19 -0800 Subject: [PATCH 095/183] Updated paramters --- eval_sim/eval_data.jl | 87 +++++++++++++++++++++++++++-- workspace/src/barc/src/LMPC_node.jl | 10 ++-- workspace/src/barc/src/barc_lib | 2 +- 3 files changed, 89 insertions(+), 10 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index d5e94409..6ebd892e 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -49,7 +49,7 @@ function eval_sim(code::AbstractString) vel_est = d_rec["vel_est"] t0 = pos_info.t[1] - track = create_track(0.3) + track = create_track(0.4) figure() ax1=subplot(311) @@ -110,7 +110,7 @@ function eval_run(code::AbstractString) pos_info = d_rec["pos_info"] t0 = pos_info.t[1] - track = create_track(0.3) + track = create_track(0.4) figure() plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*") @@ -327,7 +327,7 @@ function eval_LMPC(code::AbstractString) ylabel("Curvature") grid() - track = create_track(0.3) + track = create_track(0.4) figure() hold(1) plot(x_est[:,1],x_est[:,2],"-*") @@ -670,6 +670,7 @@ function eval_LMPC_coeff(code::AbstractString,k::Int64) sol_u = d["sol_u"] coeffCost = d["coeffCost"] coeffConst = d["coeffConst"] + cost = d["cost"] s = sol_z[:,6,k] ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] @@ -689,8 +690,47 @@ function eval_LMPC_coeff(code::AbstractString,k::Int64) grid() xlabel("s") ylabel("v_x") + println("Cost = $(cost[k,3])") end +function anim_LMPC_coeff(code::AbstractString,i::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + cost = d["cost"] + + for k=i:i+1000 + clf() + s = sol_z[:,6,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) + ylim([-0.4,0.4]) + grid() + title("Position = $(s[1]), k = $k") + xlabel("s") + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(313) + plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + ylim([0.6,1.5]) + grid() + xlabel("s") + ylabel("v_x") + println("Cost = $(cost[k,3])") + end +end + + function anim_LMPC(k1,k2) d = load(log_path_LMPC) oldTraj = d["oldTraj"] @@ -775,7 +815,7 @@ function anim_run(code::AbstractString) fig = figure(figsize=(10,10)) ax = axes(xlim = (-3,3),ylim=(-5,1)) - track = create_track(0.3) + track = create_track(0.4) plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") grid("on") car = ax[:plot]([],[],"r-+")[1] @@ -802,6 +842,45 @@ function anim_run(code::AbstractString) anim = animation.FuncAnimation(fig, animate, frames=1000, interval=50) anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); end + +function anim_constraints(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + coeffCost = d_lmpc["coeffCost"] + coeffConst = d_lmpc["coeffConst"] + + #Construct Figure and Plot Data + fig = figure(figsize=(10,10)) + ax = axes() + + pred_vx = ax[:plot]([],[],"r-+")[1] + poly_vx1 = ax[:plot]([],[],"--")[1] + poly_vx2 = ax[:plot]([],[],"--")[1] + + function init() + #car[:set_data]([],[]) + return (None) + end + function animate(k) + i = k + 1000 + s = sol_z[:,6,i] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + pred_vx[:set_data]([s],[sol_z[:,1,i]]) + poly_vx1[:set_data]([s],[ss*coeffConst[:,1,1,i]]) + poly_vx2[:set_data]([s],[ss*coeffConst[:,2,1,i]]) + + return (pred_vx,poly_vx1,poly_vx2,None) + end + t=0:30 + anim = animation.FuncAnimation(fig, animate, frames=100, interval=50) + anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); +end # ***************************************************************** # ****** HELPER FUNCTIONS ***************************************** # ***************************************************************** diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index ad042a89..8081a541 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -112,7 +112,7 @@ function main() log_t = zeros(10000,1) log_state = zeros(10000,6) log_cost = zeros(10000,6) - log_c_Vx = zeros(10000,4) + log_c_Vx = zeros(10000,5) log_c_Vy = zeros(10000,4) log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) @@ -148,9 +148,9 @@ function main() mpcSol.d_f = 0 #mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724] - mpcCoeff.c_Psi = [-0.3747957571478858,-0.005013036784512181,5.068342163488241] + #mpcCoeff.c_Psi = [-0.3747957571478858,-0.005013036784512181,5.068342163488241] #mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765] - mpcCoeff.c_Vy = [0.002968102163011754,-0.09886540158694888,0.012234790760745129,1.099308717654053] + #mpcCoeff.c_Vy = [0.002968102163011754,-0.09886540158694888,0.012234790760745129,1.099308717654053] # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) @@ -167,7 +167,7 @@ function main() uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - n_pf = 10 # number of first path-following laps (needs to be at least 2) + n_pf = 3 # number of first path-following laps (needs to be at least 2) opt_count = 0 @@ -226,7 +226,7 @@ function main() # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() - #coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) tt = toq() println("Finished coefficients, t = $tt s") end diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 1187f975..365535d6 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 1187f97584a49ecbe3b1ed7741bbaecd9ac7a1bf +Subproject commit 365535d6099b8634d93f708851b1668f91da5c55 From 37be4e67ad477f87aa2719f3378bf9f10c39e809 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Sun, 4 Dec 2016 08:17:48 -0800 Subject: [PATCH 096/183] Went back to 4-parameter xDot sysID, updated plots. --- eval_sim/eval_data.jl | 44 ++++++++++++++++--- .../src/barc/launch/sensor_test_remote.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 16 ++++--- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 6 +-- .../src/barc/src/controller_low_level.py | 6 +-- .../state_estimation_SensorKinematicModel.py | 2 +- 7 files changed, 59 insertions(+), 19 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 6ebd892e..3e9914b4 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -167,8 +167,8 @@ function eval_run(code::AbstractString) figure() title("v measurements and estimate") - plot(pos_info.t-t0,pos_info.z[:,8:9],"-*",vel_est.t-t0,vel_est.z,"-x") - legend(["est_xDot","est_yDot","v_raw"]) + plot(vel_est.t-t0,vel_est.z[:,1],"-x",pos_info.t-t0,pos_info.z[:,[8:9,4]],"-+") + legend(["v_raw","est_xDot","est_yDot","est_v"]) grid() # figure() @@ -495,6 +495,7 @@ function eval_predictions(code::AbstractString) d_lmpc = load(log_path_LMPC) t = d_lmpc["t"] + z = d_lmpc["state"] sol_z = d_lmpc["sol_z"] sol_u = d_lmpc["sol_u"] cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator @@ -505,6 +506,34 @@ function eval_predictions(code::AbstractString) N = size(sol_z,1)-1 # number of steps (prediction horizon) + #N = 7 + sz = size(t,1) + # Calculate one-step-errors: + ########### + # s + # eY + # ePsi + # v + ########### + # v_x + # v_y + # psiDot + # ePsi + # eY + # s + ########### + one_step_err = NaN*ones(sz,6) + for i=1:sz-1 + if isnan(sol_z[1,5,i]) # if we are in path following mode + one_step_err[i,1:4] = sol_z[2,1:4,i] - [z[i+1,[6,5,4]] norm(z[i+1,1:2])] + else + one_step_err[i,:] = sol_z[2,[6,5,4,1,2,3],i] - z[i+1,[6,5,4,1,2,3]] + end + end + i=1 + while isnan(sol_z[1,5,i]) + i = i+1 + end figure(1) plot(t-t0,step_diff) grid("on") @@ -574,10 +603,11 @@ function eval_predictions(code::AbstractString) end grid("on") + N = 7 subplot(414,sharex=ax1) title("Open loop inputs") for i=1:2:size(t,1)-N - plot(t[i:i+N-1]-t0,sol_u[1:N,2,i],t[i:i+N-1]-t0,sol_u[:,1,i]) + plot(t[i:i+N-3]-t0,sol_u[1:N-2,2,i],t[i:i+N-1]-t0,sol_u[1:N,1,i]) end grid("on") end @@ -693,7 +723,7 @@ function eval_LMPC_coeff(code::AbstractString,k::Int64) println("Cost = $(cost[k,3])") end -function anim_LMPC_coeff(code::AbstractString,i::Int64) +function anim_LMPC_coeff(code::AbstractString) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" d = load(log_path_LMPC) oldTraj = d["oldTraj"] @@ -703,7 +733,11 @@ function anim_LMPC_coeff(code::AbstractString,i::Int64) coeffConst = d["coeffConst"] cost = d["cost"] - for k=i:i+1000 + i=1 + while isnan(sol_z[1,5,i]) + i = i+1 + end + for k=i:size(cost,1) clf() s = sol_z[:,6,k] ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index 6349fd58..16c3c5eb 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -12,7 +12,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 8081a541..2f2eba9b 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -47,7 +47,7 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: oldTraj.count[lapStatus.currentLap] += 1 # if necessary: append to end of previous lap - if lapStatus.currentLap > 1 && z_est[6] < 8.0 + if lapStatus.currentLap > 1 && z_est[6] < 12.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] @@ -57,7 +57,7 @@ function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol: end #if necessary: append to beginning of next lap - if z_est[6] > posInfo.s_target - 8.0 + if z_est[6] > posInfo.s_target - 12.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] @@ -112,7 +112,7 @@ function main() log_t = zeros(10000,1) log_state = zeros(10000,6) log_cost = zeros(10000,6) - log_c_Vx = zeros(10000,5) + log_c_Vx = zeros(10000,4) log_c_Vy = zeros(10000,4) log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) @@ -191,9 +191,9 @@ function main() # ============================= Pre-Logging (before solving) ================================ log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) if size(mpcSol.z,2) == 4 # find 1-step-error - step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]).^2 + step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]) else - step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]).^2 + step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]) end log_step_diff[k+1,:] = step_diff @@ -211,6 +211,12 @@ function main() # Set warm start for new solution (because s shifted by s_target) if lapStatus.currentLap <= n_pf setvalue(mdl_pF.z_Ol[:,1],mpcSol.z[:,1]-posInfo.s_target) + elseif lapStatus.currentLap == n_pf+1 + setvalue(mdl.z_Ol[:,1],mpcSol.z[1:mpcParams.N+1,4]) + setvalue(mdl.z_Ol[:,6],mpcSol.z[1:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl.z_Ol[:,5],mpcSol.z[1:mpcParams.N+1,2]) + setvalue(mdl.z_Ol[:,4],mpcSol.z[1:mpcParams.N+1,3]) + setvalue(mdl.u_Ol,mpcSol.u[1:mpcParams.N,:]) elseif lapStatus.currentLap > n_pf+1 setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) end diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 365535d6..2043df32 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 365535d6099b8634d93f708851b1668f91da5c55 +Subproject commit 2043df322c014c84be056994bfb5f9ab84fa568c diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index dbb05b9d..95fe092a 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -123,9 +123,9 @@ function main() while ! is_shutdown() t_ros = get_rostime() t = to_sec(t_ros) - #if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 - # u_current[2] = cmd_log.z[t.>cmd_log.t,2][end] # artificial steering input delay - #end + # if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 + # u_current[2] = cmd_log.z[t.>=cmd_log.t+0.2,2][end] # artificial steering input delay + # end # update current state with a new row vector z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:], u_current', dt, modelParams) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 947bba5f..af23aa42 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -49,9 +49,9 @@ def pwm_converter_callback(self, msg): if FxR == 0: self.motor_pwm = 90.0 elif FxR > 0: - #self.motor_pwm = 94.14 + 2.7678*FxR # using write() in Arduino - #self.motor_pwm = 87.6 + 4.83*FxR # using writeMicroseconds() in Arduino (old) - self.motor_pwm = max(93,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down self.motor_pwm = 93.5 + 46.73*FxR diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 450fab91..9545add2 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -193,7 +193,7 @@ def state_estimation(): #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.01,0.01,1.0,1.0,0.1]) - R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 1.0,1.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters From 22a5af06727adad4fc52a6705f913f930aeaf13d Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Sun, 4 Dec 2016 09:27:59 -0800 Subject: [PATCH 097/183] Added filtered state in LMPC and evaluation --- eval_sim/eval_data.jl | 24 ++++++++++++++++--- workspace/src/barc/msg/pos_info.msg | 1 + workspace/src/barc/src/LMPC_node.jl | 12 +++++----- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_record.jl | 2 +- .../state_estimation_SensorKinematicModel.py | 23 ++++++++++-------- 6 files changed, 43 insertions(+), 21 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 3e9914b4..30fca902 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -112,6 +112,14 @@ function eval_run(code::AbstractString) t0 = pos_info.t[1] track = create_track(0.4) + # Calculate accelerations + acc = smooth(diff(smooth(vel_est.z[:,1],10))./diff(vel_est.t),10) + figure(10) + plot(vel_est.t[1:end-1]-t0,acc,cmd_log.t-t0,cmd_log.z[:,1]) + grid("on") + legend(["Acc","u_a"]) + + figure() plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*") plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") @@ -137,7 +145,7 @@ function eval_run(code::AbstractString) figure() ax2=subplot(211) title("Commands") - plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x") + plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x",pos_info.t-t0,pos_info.z[:,21]) grid("on") xlabel("t [s]") subplot(212,sharex=ax2) @@ -276,8 +284,8 @@ function eval_LMPC(code::AbstractString) xlabel("t [s]") ylabel("v_x [m/s]") subplot(312,sharex=ax1) - plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*") - legend(["a_rec","d_f_rec","a_MPC","d_f_MPC"]) + plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*",t-t0,state[:,7]) + legend(["a_rec","d_f_rec","a_MPC","d_f_MPC","acc_filter"]) grid("on") subplot(313,sharex=ax1) plot(t-t0,c_Vx) @@ -1105,4 +1113,14 @@ function checkConnectivity(code::AbstractString) plot(gps_meas.t,gps_meas.z,"-x",gps_meas.t_msg,gps_meas.z,"--x",pos_info.t,pos_info.z[:,12:13],"-*",pos_info.t_msg,pos_info.z[:,12:13],"--*") grid("on") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y end \ No newline at end of file diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 52d3c3a1..881e827d 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -12,6 +12,7 @@ float64 v_x float64 v_y float64 psi float64 psiDot +float64 acc_f float64 x_raw float64 y_raw float64 psi_raw diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 2f2eba9b..017b35f2 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -24,7 +24,7 @@ include("barc_lib/simModel.jl") # It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,trackCoeff::TrackCoeff,z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data # update mpc initial condition - z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # use z_est as pointer + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,msg.acc_f] # use z_est as pointer x_est[:] = [msg.x,msg.y,msg.psi,msg.v] trackCoeff.coeffCurvature = msg.coeffCurvature @@ -93,7 +93,7 @@ function main() max_N = max(mpcParams.N,mpcParams_pF.N) # ROS-specific variables - z_est = zeros(6) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) + z_est = zeros(7) # this is a buffer that saves current state information (xDot, yDot, psiDot, ePsi, eY, s) x_est = zeros(4) # this is a buffer that saves further state information (x, y, psi, v) coeffX = zeros(9) # buffer for coeffX (only logging) coeffY = zeros(9) # buffer for coeffY (only logging) @@ -103,14 +103,14 @@ function main() # Logging variables log_coeff_Cost = NaN*ones(mpcCoeff.order+1,2,10000) log_coeff_Const = NaN*ones(mpcCoeff.order+1,2,5,10000) - log_sol_z = NaN*ones(max_N+1,6,10000) + log_sol_z = NaN*ones(max_N+1,7,10000) log_sol_u = NaN*ones(max_N,2,10000) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) log_state_x = zeros(10000,4) log_coeffX = zeros(10000,9) log_coeffY = zeros(10000,9) log_t = zeros(10000,1) - log_state = zeros(10000,6) + log_state = zeros(10000,7) log_cost = zeros(10000,6) log_c_Vx = zeros(10000,4) log_c_Vy = zeros(10000,4) @@ -132,7 +132,7 @@ function main() println("Finished initialization.") # buffer in current lap - zCurr = zeros(10000,6) # contains state information in current lap (max. 10'000 steps) + zCurr = zeros(10000,7) # contains state information in current lap (max. 10'000 steps) uCurr = zeros(10000,2) # contains input information step_diff = zeros(5) @@ -292,7 +292,7 @@ function main() log_sol_z[1:mpcParams_pF.N+1,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) log_sol_u[1:mpcParams_pF.N,:,k] = mpcSol.u else - log_sol_z[1:mpcParams.N+1,1:6,k] = mpcSol.z + log_sol_z[1:mpcParams.N+1,1:7,k] = mpcSol.z log_sol_u[1:mpcParams.N,:,k] = mpcSol.u end diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 2043df32..55673bb7 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 2043df322c014c84be056994bfb5f9ab84fa568c +Subproject commit 55673bb7293265300dfe7f9e920d991fb39c9db7 diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 976121e9..c5a0a118 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -34,7 +34,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,20)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,21)) # initiate node, set up publisher / subscriber topics init_node("barc_record") diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 9545add2..d61f8704 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -186,8 +186,8 @@ def state_estimation(): z_EKF = zeros(14) # x, y, psi, v, psi_drift P = eye(14) # initial dynamics coveriance matrix - qa = 1000 - qp = 1000 + qa = 1000.0 + qp = 1000.0 # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v #Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) @@ -201,19 +201,20 @@ def state_estimation(): l.create_track() l.prepare_trajectory(0.06) - d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag - d_f_lp = 0 - a_lp = 0 + d_f_hist = [0.0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_f_lp = 0.0 + a_lp = 0.0 - t_now = 0 + t_now = 0.0 # Estimation variables (x_est, y_est, a_x_est, a_y_est) = [0]*4 - bta = 0 - v_est = 0 - psi_est = 0 + bta = 0.0 + v_est = 0.0 + psi_est = 0.0 est_counter = 0 + acc_f = 0.0 while not rospy.is_shutdown(): t_now = rospy.get_rostime().to_sec()-se.t0 @@ -254,6 +255,8 @@ def state_estimation(): u = [se.cmd_motor, d_f_hist.pop(0)] bta = 0.5 * u[1] + acc_f = acc_f + dt*(se.cmd_motor-acc_f)*0.5 + # get measurement y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) @@ -283,7 +286,7 @@ def state_estimation(): # and then publish position info ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas, + l.psi, l.psiDot, acc_f, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas, psi_drift_est, a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, (0,), (0,), (0,), l.coeffCurvature.tolist())) From cb8e50628c1656a0c67421eff1515cae5785d2f9 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Sun, 4 Dec 2016 12:53:11 -0800 Subject: [PATCH 098/183] Fixed filter formulation. --- eval_sim/eval_data.jl | 25 ++++++++++++++----- workspace/src/barc/msg/pos_info.msg | 1 - workspace/src/barc/src/LMPC_node.jl | 20 ++++++++++----- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/barc_record.jl | 2 +- .../state_estimation_SensorKinematicModel.py | 5 ++-- 6 files changed, 37 insertions(+), 18 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 30fca902..a6a15c7e 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -145,7 +145,7 @@ function eval_run(code::AbstractString) figure() ax2=subplot(211) title("Commands") - plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x",pos_info.t-t0,pos_info.z[:,21]) + plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x") grid("on") xlabel("t [s]") subplot(212,sharex=ax2) @@ -740,34 +740,47 @@ function anim_LMPC_coeff(code::AbstractString) coeffCost = d["coeffCost"] coeffConst = d["coeffConst"] cost = d["cost"] + sol_status = d_lmpc["sol_status"] i=1 - while isnan(sol_z[1,5,i]) + while isnan(sol_z[1,6,i]) i = i+1 end for k=i:size(cost,1) clf() s = sol_z[:,6,k] ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - subplot(311) + subplot(511) plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) ylim([-0.4,0.4]) grid() - title("Position = $(s[1]), k = $k") + title("Position = $(s[1]), k = $k, status = $(sol_status[k])") xlabel("s") ylabel("e_Y") - subplot(312) + subplot(512) plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) ylim([-0.5,0.5]) grid() xlabel("s") ylabel("e_Psi") - subplot(313) + subplot(513) + plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("psiDot") + subplot(514) plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) ylim([0.6,1.5]) grid() xlabel("s") ylabel("v_x") + subplot(515) + plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("v_y") println("Cost = $(cost[k,3])") end end diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg index 881e827d..52d3c3a1 100644 --- a/workspace/src/barc/msg/pos_info.msg +++ b/workspace/src/barc/msg/pos_info.msg @@ -12,7 +12,6 @@ float64 v_x float64 v_y float64 psi float64 psiDot -float64 acc_f float64 x_raw float64 y_raw float64 psi_raw diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 017b35f2..226ffaef 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -22,9 +22,9 @@ include("barc_lib/simModel.jl") # This function is called whenever a new state estimate is received. # It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) -function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,trackCoeff::TrackCoeff,z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data +function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,trackCoeff::TrackCoeff,z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data # update mpc initial condition - z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,msg.acc_f] # use z_est as pointer + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # use z_est as pointer x_est[:] = [msg.x,msg.y,msg.psi,msg.v] trackCoeff.coeffCurvature = msg.coeffCurvature @@ -120,12 +120,14 @@ function main() log_t_solv = zeros(10000) log_sol_status = Array(Symbol,10000) + acc_f = [0.0] + # Initialize ROS node and topics init_node("mpc_traj") loop_rate = Rate(1/modelParams.dt) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: - s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,posInfo,mpcSol,oldTraj,trackCoeff,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} + s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,trackCoeff,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! run_id = get_param("run_id") @@ -169,6 +171,7 @@ function main() n_pf = 3 # number of first path-following laps (needs to be at least 2) + acc0 = 0.0 opt_count = 0 # Start node @@ -241,11 +244,16 @@ function main() # Solve the MPC problem tic() if lapStatus.currentLap <= n_pf - z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2])] # use kinematic model and its states + z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2]),acc0] # use kinematic model and its states solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) + acc_f[1] = mpcSol.z[1,5] + acc0 = mpcSol.z[2,5] else # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) + zCurr[i,7] = acc0 solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) + acc0 = mpcSol.z[2,7] + acc_f[1] = mpcSol.z[1,7] end log_t_solv[k+1] = toq() @@ -288,8 +296,8 @@ function main() log_c_Vx[k,:] = mpcCoeff.c_Vx log_c_Vy[k,:] = mpcCoeff.c_Vy log_c_Psi[k,:] = mpcCoeff.c_Psi - if size(mpcSol.z,2) == 4 - log_sol_z[1:mpcParams_pF.N+1,1:4,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) + if size(mpcSol.z,2) == 5 + log_sol_z[1:mpcParams_pF.N+1,1:5,k] = mpcSol.z # only 4 states during path following mode (first 2 laps) log_sol_u[1:mpcParams_pF.N,:,k] = mpcSol.u else log_sol_z[1:mpcParams.N+1,1:7,k] = mpcSol.z diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 55673bb7..957d940c 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 55673bb7293265300dfe7f9e920d991fb39c9db7 +Subproject commit 957d940c6dce38f469d92b7825fcc017f6485be3 diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index c5a0a118..976121e9 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -34,7 +34,7 @@ function main() cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) - pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,21)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,20)) # initiate node, set up publisher / subscriber topics init_node("barc_record") diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index d61f8704..0108fad7 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -193,7 +193,7 @@ def state_estimation(): #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.01,0.01,1.0,1.0,0.1]) - R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 1.0,1.0]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters @@ -255,7 +255,6 @@ def state_estimation(): u = [se.cmd_motor, d_f_hist.pop(0)] bta = 0.5 * u[1] - acc_f = acc_f + dt*(se.cmd_motor-acc_f)*0.5 # get measurement y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, @@ -286,7 +285,7 @@ def state_estimation(): # and then publish position info ros_t = rospy.get_rostime() state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, acc_f, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas, psi_drift_est, a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, (0,), (0,), (0,), l.coeffCurvature.tolist())) From f12150374b6559fd50737b85493727b6db36fb4d Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 5 Dec 2016 08:19:18 -0800 Subject: [PATCH 099/183] Last optimizations. --- eval_sim/Kalman simulation/sim_kalman_imu2.jl | 34 +++++++++---------- eval_sim/create_track.py | 21 +++++++++--- eval_sim/eval_data.jl | 19 ++++++++--- workspace/src/barc/src/LMPC_node.jl | 20 +++++------ .../src/barc/src/Localization_helpers.py | 25 ++++++++++---- workspace/src/barc/src/barc_lib | 2 +- .../state_estimation_SensorKinematicModel.py | 29 +++------------- 7 files changed, 79 insertions(+), 71 deletions(-) diff --git a/eval_sim/Kalman simulation/sim_kalman_imu2.jl b/eval_sim/Kalman simulation/sim_kalman_imu2.jl index 3f3536a1..ec7a24f0 100644 --- a/eval_sim/Kalman simulation/sim_kalman_imu2.jl +++ b/eval_sim/Kalman simulation/sim_kalman_imu2.jl @@ -53,9 +53,12 @@ function main(code::AbstractString) # x, y, vx, vy, ax, ay, psi, psidot, psidrift Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1]) #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) - R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,10.0, 10.0,10.0]) # x, y, v, psi, psidot, ax, a_y x, y, psi, v, v_x, v_y + y_vel_prev = 0.0 + y_vel_est = 0.0 + for i=2:length(t) # Collect measurements and inputs for this iteration # Interpolate GPS-measurements: @@ -93,12 +96,18 @@ function main(code::AbstractString) a_y = acc_f[2] #y_yawdot = rot_f[3] - #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] - y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] - u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end] + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] + if pos_info.z[t[i].>pos_info.t,15][end] != y_vel_prev + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + y_vel_prev = y_vel_est + else + y_vel_est = y_vel_est + dt*(u[i,1] - 0.5*x_est_gps_imu[i-1,13]) + end + + bta = atan(0.5*tan(u[i,2])) y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) @@ -107,26 +116,15 @@ function main(code::AbstractString) args = (u[i,:],dt,l_A,l_B) - # if y_gps_imu[i,3] == y_gps_imu[i-1,3] # if no new velocity measurement - # R_gps_imu[2,2] = 1.0 - # R_gps_imu[10,10] = 1.0 - # else - # R_gps_imu[2,2] = 1.0#0.1 - # R_gps_imu[10,10] = 1.0#0.1 - # end # Calculate new estimate (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) end - #figure(5) - #plot(t-t0,y_gps_imu) - #grid("on") - println("Yaw0 = $yaw0") figure(1) plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") plot(t-t0,x_est_gps_imu[:,10:11],"-x") - plot(pos_info.t-t0,pos_info.z[:,12:13],"..+") + #plot(pos_info.t-t0,pos_info.z[:,12:13],"-+") #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") grid("on") legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) @@ -140,9 +138,9 @@ function main(code::AbstractString) figure(3) v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,[3:4,13]],"--",vel_est.t-t0,vel_est.z[:,1]) plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") - legend(["v_est","v_x_est","v_y_est","v_meas","v_x_onboard","v_y_onboard"]) + legend(["v_est","v_x_est","v_y_est","v_kin_est","v_meas","v_x_onboard","v_y_onboard"]) grid("on") title("Velocity estimate and measurement") diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index 68caee35..d5ecfcde 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -50,12 +50,23 @@ def add_curve(theta, length, angle): # theta = add_curve(theta,40,-pi/2) # theta = add_curve(theta,35,0) +# SIMPLE GOGGLE TRACK +theta = add_curve(theta,30,0) +theta = add_curve(theta,40,-pi/2) +theta = add_curve(theta,40,-pi/2) +theta = add_curve(theta,20,-pi/10) +theta = add_curve(theta,30,pi/5) +theta = add_curve(theta,20,-pi/10) +theta = add_curve(theta,40,-pi/2) +theta = add_curve(theta,40,-pi/2) +theta = add_curve(theta,38,0) + # SIMPLE TRACK -theta = add_curve(theta,10,0) -theta = add_curve(theta,80,-pi) -theta = add_curve(theta,20,0) -theta = add_curve(theta,80,-pi) -theta = add_curve(theta,9,0) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,80,-pi) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi) +# theta = add_curve(theta,9,0) for i in range(0,size(theta)): x = hstack((x, x[-1] + cos(theta[i])*ds)) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index a6a15c7e..3da4cc07 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -740,7 +740,7 @@ function anim_LMPC_coeff(code::AbstractString) coeffCost = d["coeffCost"] coeffConst = d["coeffConst"] cost = d["cost"] - sol_status = d_lmpc["sol_status"] + sol_status = d["sol_status"] i=1 while isnan(sol_z[1,6,i]) @@ -1008,12 +1008,23 @@ function create_track(w) # add_curve(theta,49,0) # GOGGLE TRACK + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,-pi/6) + # add_curve(theta,30,pi/3) + # add_curve(theta,20,-pi/6) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + # SIMPLE GOGGLE TRACK add_curve(theta,30,0) add_curve(theta,40,-pi/2) add_curve(theta,40,-pi/2) - add_curve(theta,20,-pi/6) - add_curve(theta,30,pi/3) - add_curve(theta,20,-pi/6) + add_curve(theta,20,-pi/10) + add_curve(theta,30,pi/5) + add_curve(theta,20,-pi/10) add_curve(theta,40,-pi/2) add_curve(theta,40,-pi/2) add_curve(theta,35,0) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 226ffaef..f2f18c3d 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -141,7 +141,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 17.76#24.0 + posInfo.s_target = 17.94#17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -202,9 +202,7 @@ function main() # ======================================= Lap trigger ======================================= if lapStatus.nextLap # if we are switching to the next lap... - println("Finishing one lap at iteration $i") - println("current state: ", zCurr[i,:]) - println("previous state: ", zCurr[i-1,:]) + println("Finishing one lap at iteration ",i) # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj zCurr[1,:] = zCurr[i,:] # copy current state i = 1 @@ -226,21 +224,21 @@ function main() end # ======================================= Calculate input ======================================= - println("=================================== NEW ITERATION # $i ===================================") + #println("*** NEW ITERATION # ",i," ***") println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt) - println("State Nr. ", i, " = ", z_est) - println("s = $(posInfo.s)") - println("s_total = $(posInfo.s%posInfo.s_target)") + #println("State Nr. ", i, " = ", z_est) + #println("s = $(posInfo.s)") + #println("s_total = $(posInfo.s%posInfo.s_target)") # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) tt = toq() - println("Finished coefficients, t = $tt s") + println("Finished coefficients, t = ",tt," s") end - println("Starting solving.") + #println("Starting solving.") # Solve the MPC problem tic() if lapStatus.currentLap <= n_pf @@ -280,7 +278,7 @@ function main() uPrev = circshift(uPrev,1) uPrev[1,:] = uCurr[i,:] - println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $(log_t_solv[k+1]) s") + #println("Finished solving, status: $(mpcSol.solverStatus), u = $(uCurr[i,:]), t = $(log_t_solv[k+1]) s") # Logging # --------------------------- diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 0f1c372d..1494441e 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -40,10 +40,10 @@ class Localization(object): psi = 0 # current orientation nodes = array([0]) # all nodes are saved in a matrix N_nodes_poly_back = 20 # number of nodes behind current position - N_nodes_poly_front = 40 # number of nodes in front + N_nodes_poly_front = 60 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 6 # order of x-y-polynomial interpolation + OrderXY = 9 # order of x-y-polynomial interpolation OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? @@ -140,16 +140,27 @@ def create_track(self): # theta = add_curve(theta,100,-pi) # theta = add_curve(theta,49,0) - # GOGGLE TRACK: length = 17.76m + # AGGRESSIVE GOGGLE TRACK: length = 17.76m + # theta = add_curve(theta,30,0) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,20,-pi/6) + # theta = add_curve(theta,30,pi/3) + # theta = add_curve(theta,20,-pi/6) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,35,0) + + # SIMPLE GOGGLE TRACK: length = 17.94m theta = add_curve(theta,30,0) theta = add_curve(theta,40,-pi/2) theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,20,-pi/6) - theta = add_curve(theta,30,pi/3) - theta = add_curve(theta,20,-pi/6) + theta = add_curve(theta,20,-pi/10) + theta = add_curve(theta,30,pi/5) + theta = add_curve(theta,20,-pi/10) theta = add_curve(theta,40,-pi/2) theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,35,0) + theta = add_curve(theta,38,0) # SHORT SIMPLE RACETRACK (smooth curves): 12.0m # theta = add_curve(theta,10,0) diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 957d940c..bca11ed0 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 957d940c6dce38f469d92b7825fcc017f6485be3 +Subproject commit bca11ed0c9d16b0ebda5010c6eebb066033884c2 diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 0108fad7..290cd566 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -192,7 +192,7 @@ def state_estimation(): #Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) - Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.01,0.01,1.0,1.0,0.1]) + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v @@ -208,39 +208,18 @@ def state_estimation(): t_now = 0.0 # Estimation variables - (x_est, y_est, a_x_est, a_y_est) = [0]*4 + (x_est, y_est, a_x_est, a_y_est, v_est_2) = [0]*5 bta = 0.0 v_est = 0.0 psi_est = 0.0 est_counter = 0 acc_f = 0.0 + vel_meas_est = 0.0 while not rospy.is_shutdown(): t_now = rospy.get_rostime().to_sec()-se.t0 - # make R values dependent on current measurement (robust against outliers) - # sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - # if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: - # R[0,0] = 1.0 - # R[1,1] = 1.0 - # else: - # # otherwise just extrapolate measurements: - # #se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) - # #se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) - # R[0,0] = 10.0 - # R[1,1] = 10.0 - # if se.imu_updated: - # R[3,3] = 1.0 - # R[4,4] = 5.0 - # else: - # R[3,3] = 10.0 - # R[4,4] = 50.0 - # if se.vel_updated: - # R[2, 2] = 1.0 - # R[10, 10] = 1.0 - # else: - # R[2, 2] = 1.0 - # R[10, 10] = 1.0 + se.x_meas = polyval(se.c_X, t_now) se.y_meas = polyval(se.c_Y, t_now) se.gps_updated = False From f2b84424039290dd12a6c057952b99222b7248b0 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Mon, 5 Dec 2016 19:18:00 -0800 Subject: [PATCH 100/183] Updated localization (decreased polynomial to 3rd order), added debugging script. Updated parameters. --- eval_sim/create_track.py | 35 +++++--- eval_sim/eval_data.jl | 10 ++- workspace/src/barc/src/LMPC_node.jl | 6 +- .../src/barc/src/Localization_helpers.py | 87 +++++++++++-------- workspace/src/barc/src/barc_lib | 2 +- .../src/barc/src/controller_low_level.py | 4 +- workspace/src/barc/src/debug_localization.py | 31 +++++++ .../state_estimation_SensorKinematicModel.py | 2 +- 8 files changed, 118 insertions(+), 59 deletions(-) create mode 100644 workspace/src/barc/src/debug_localization.py diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py index d5ecfcde..2659b37a 100644 --- a/eval_sim/create_track.py +++ b/eval_sim/create_track.py @@ -3,7 +3,7 @@ x = array([0]) # starting point y = array([0]) -ds = 0.06 +ds = 0.03 theta = 0 d_theta = 0 @@ -51,15 +51,30 @@ def add_curve(theta, length, angle): # theta = add_curve(theta,35,0) # SIMPLE GOGGLE TRACK -theta = add_curve(theta,30,0) -theta = add_curve(theta,40,-pi/2) -theta = add_curve(theta,40,-pi/2) -theta = add_curve(theta,20,-pi/10) -theta = add_curve(theta,30,pi/5) -theta = add_curve(theta,20,-pi/10) -theta = add_curve(theta,40,-pi/2) -theta = add_curve(theta,40,-pi/2) -theta = add_curve(theta,38,0) +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,20,pi/10) +# theta = add_curve(theta,30,-pi/5) +# theta = add_curve(theta,20,pi/10) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,37,0) + +# SIMPLE GOGGLE TRACK +# theta = add_curve(theta,60,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,40,pi/10) +# theta = add_curve(theta,60,-pi/5) +# theta = add_curve(theta,40,pi/10) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,75,0) # SIMPLE TRACK # theta = add_curve(theta,10,0) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 3da4cc07..14e1c68b 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -317,17 +317,19 @@ function eval_LMPC(code::AbstractString) c = zeros(size(curv,1),1) for i=1:size(curv,1) s = state[i,6] - c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] end plot(state[:,6],c,"-o") - for i=1:1:size(curv,1) - if sol_z[1,5,i] == 0 + for i=1:5:size(curv,1) + if isnan(sol_z[1,6,i]) s = sol_z[:,1,i] else s = sol_z[:,6,i] end c = zeros(size(curv,1),1) - c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]' plot(s,c,"-*") end title("Curvature over path") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index f2f18c3d..70feafc1 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -29,7 +29,7 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po trackCoeff.coeffCurvature = msg.coeffCurvature # check if lap needs to be switched - if z_est[6] % posInfo.s_target <= lapStatus.s_lapTrigger && lapStatus.switchLap + if z_est[6] <= lapStatus.s_lapTrigger && lapStatus.switchLap oldTraj.idx_end[lapStatus.currentLap] = oldTraj.count[lapStatus.currentLap] oldTraj.oldCost[lapStatus.currentLap] = oldTraj.idx_end[lapStatus.currentLap] - oldTraj.idx_start[lapStatus.currentLap] lapStatus.currentLap += 1 @@ -141,7 +141,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 17.94#17.76#24.0 + posInfo.s_target = 19.11#19.14#17.94#17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -159,7 +159,7 @@ function main() oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) posInfo.s = posInfo.s_target/2 lapStatus.currentLap = 3 - oldTraj.count[3] = 200 + oldTraj.count[3] = 500 coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) oldTraj.count[3] = 1 lapStatus.currentLap = 1 diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 1494441e..1b207231 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -40,15 +40,14 @@ class Localization(object): psi = 0 # current orientation nodes = array([0]) # all nodes are saved in a matrix N_nodes_poly_back = 20 # number of nodes behind current position - N_nodes_poly_front = 60 # number of nodes in front + N_nodes_poly_front = 50 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 9 # order of x-y-polynomial interpolation - OrderThetaCurv = 8 # order of theta interpolation + OrderXY = 4 # order of x-y-polynomial interpolation + OrderThetaCurv = 3 # order of theta interpolation closed = True # open or closed trajectory? - coeffTheta = zeros(9) - coeffCurvature = zeros(9) + coeffCurvature = zeros(4) s = 0 # distance from s_start to current closest node (idx_min) s_start = 0 # distance along path from first node to start node (which is N_nodes_poly_back behind current closest node) @@ -113,7 +112,7 @@ def create_track2(self): def create_track(self): x = array([0]) # starting point y = array([0]) - ds = 0.06 + ds = 0.03 theta = array([0]) # Sophisticated racetrack: length = 25.62m @@ -134,11 +133,11 @@ def create_track(self): # theta = add_curve(theta,28,0) # # SIMPLE RACETRACK (smooth curves): length = 24.0m - # theta = add_curve(theta,50,0) - # theta = add_curve(theta,100,-pi) - # theta = add_curve(theta,100,0) - # theta = add_curve(theta,100,-pi) - # theta = add_curve(theta,49,0) + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,80,-pi) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi) + # theta = add_curve(theta,9,0) # AGGRESSIVE GOGGLE TRACK: length = 17.76m # theta = add_curve(theta,30,0) @@ -152,15 +151,30 @@ def create_track(self): # theta = add_curve(theta,35,0) # SIMPLE GOGGLE TRACK: length = 17.94m - theta = add_curve(theta,30,0) - theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,20,-pi/10) - theta = add_curve(theta,30,pi/5) - theta = add_curve(theta,20,-pi/10) - theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,40,-pi/2) - theta = add_curve(theta,38,0) + # theta = add_curve(theta,30,0) + # theta = add_curve(theta,40,-pi/2) + # #theta = add_curve(theta,10,0) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,20,pi/10) + # theta = add_curve(theta,30,-pi/5) + # theta = add_curve(theta,20,pi/10) + # theta = add_curve(theta,40,-pi/2) + # #theta = add_curve(theta,10,0) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,37,0) + + # GOGGLE TRACK WITH STRAIGHT LINES, LENGTH = 19.11m (using ds = 0.03m) + theta = add_curve(theta,60,0) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,20,0) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,40,pi/10) + theta = add_curve(theta,60,-pi/5) + theta = add_curve(theta,40,pi/10) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,20,0) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,75,0) # SHORT SIMPLE RACETRACK (smooth curves): 12.0m # theta = add_curve(theta,10,0) @@ -179,7 +193,7 @@ def create_track(self): self.ds = ds self.n = size(x) print "number of nodes: %i"%self.n - print "length : %f"%((self.n-1)*ds) + print "length : %f"%((self.n)*ds) def create_racetrack(self,L=1.0,b=1.0,ds=0.5,c=array([0,0]),ang=0): # problem: points are not equidistant at connecing points x = linspace(0,L/2.0,5)#arange(0,L/2.0,ds) # otherwise: would create a racetrack with parallel lines @@ -238,12 +252,6 @@ def prepare_trajectory(self,ds_in): # brings trajectory to correct format (e xn = interp(sn,s,x) yn = interp(sn,s,y) - # if length - sn[-1] < dsn: - # sn = sn[0:n-1] - # xn = xn[0:n-1] - # yn = yn[0:n-1] - # n = n - 1 - self.nodes = array([xn,yn]) self.ds = dsn self.n = size(xn) @@ -268,23 +276,31 @@ def set_pos(self,x,y,psi,v_x,v_y,psiDot): def find_s(self): dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of current position to all nodes idx_min = argmin(dist) # index of minimum distance - # print "closest point: %f"%idx_min + n = self.n # number of nodes nPoints = self.nPoints # number of points for polynomial approximation (around current position) - + # Use closest node to determine start and end of polynomial approximation idx_start = idx_min - self.N_nodes_poly_back idx_end = idx_min + self.N_nodes_poly_front + n_poly = self.N_nodes_poly_back + self.N_nodes_poly_front + 1 + s_start = idx_start * self.ds + if self.closed == True: # if the track is modeled as closed (start = end) if idx_start<0: # and if the start for polynomial approx. is before the start line nodes_X = hstack((self.nodes[0,n+idx_start:n],self.nodes[0,0:idx_end+1])) # then stack the end and beginning of a lap together nodes_Y = hstack((self.nodes[1,n+idx_start:n],self.nodes[1,0:idx_end+1])) + #nodes_X = hstack((linspace(idx_start,-1,-idx_start)*self.ds,self.nodes[0,0:idx_end+1])) + #nodes_Y = hstack((zeros(-idx_start),self.nodes[1,0:idx_end+1])) + idx_start = n+idx_start elif idx_end>n-1: # if the end is behind the finish line nodes_X = hstack((self.nodes[0,idx_start:n],self.nodes[0,0:idx_end+1-n])) # then stack the end and beginning of the lap together nodes_Y = hstack((self.nodes[1,idx_start:n],self.nodes[1,0:idx_end+1-n])) + #nodes_X = hstack((self.nodes[0,idx_start:n],self.nodes[0,n-1]+linspace(1,idx_end-n+1,idx_end-n+1)*self.ds)) + #nodes_Y = hstack((self.nodes[1,idx_start:n],ones(idx_end-n+1)*self.nodes[1,n-1])) else: # if we are somewhere in the middle of the track nodes_X = self.nodes[0,idx_start:idx_end+1] # then just use the nodes from idx_start to end for interpolation nodes_Y = self.nodes[1,idx_start:idx_end+1] @@ -334,18 +350,10 @@ def find_s(self): dY = polyval(pdcy1,s) ddX = polyval(pdcx2,s) ddY = polyval(pdcy2,s) - # angle = arctan2(dY,dX) - # if j>1: # unwrap angles - # if angle - b_theta_vec[j-1] > pi: - # angle = angle - 2*pi - # elif angle - b_theta_vec[j-1] < -pi: - # angle = angle + 2*pi - # b_theta_vec[j] = angle b_curvature_vector[j] = (dX*ddY-dY*ddX)/(dX**2+dY**2)**1.5 # this calculates the curvature values for all points in the interp. interval # these values are going to be approximated by a polynomial! - # calculate coefficients for Theta and curvature - #coeffTheta = linalg.lstsq(Matrix3rd,b_theta_vec)[0] # not needed + # calculate coefficients for curvature coeffCurvature = linalg.lstsq(Matrix3rd,b_curvature_vector)[0] # Calculate s @@ -375,6 +383,9 @@ def find_s(self): epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle epsi = (epsi+pi)%(2*pi)-pi + if s < 0.0: + s = s + self.n*self.ds + self.epsi = epsi self.ey = ey self.s = s diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index bca11ed0..26a0ca75 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit bca11ed0c9d16b0ebda5010c6eebb066033884c2 +Subproject commit 26a0ca7591f8516d4198a54e2074434f28425b2e diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index af23aa42..789c5386 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -49,9 +49,9 @@ def pwm_converter_callback(self, msg): if FxR == 0: self.motor_pwm = 90.0 elif FxR > 0: - #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino - self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down self.motor_pwm = 93.5 + 46.73*FxR diff --git a/workspace/src/barc/src/debug_localization.py b/workspace/src/barc/src/debug_localization.py new file mode 100644 index 00000000..76fb72d3 --- /dev/null +++ b/workspace/src/barc/src/debug_localization.py @@ -0,0 +1,31 @@ +from Localization_helpers import Localization +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append +from numpy import ones, polyval, delete, size, empty, linspace +import math +import matplotlib.pyplot as plt + +l = Localization() +l.create_track() + +plt.plot(l.nodes[0,:],l.nodes[1,:],"r-o") +plt.grid('on') +plt.axis('equal') +plt.show() + + +n = size(l.nodes[0,:]) +#c = empty([n,10]) +ss = empty(n) +for i in range(0,n): + l.set_pos(l.nodes[0,i], l.nodes[1,i], 0.0,0.0,0.0,0.0) + l.find_s() + s = linspace(l.s,l.s+1.0,5) + ss[i] = l.s + #c[i,:] = polyval(l.coeffCurvature,s) + c = polyval(l.coeffCurvature,s) + plt.plot(s,c) + print l.s + #print c + +#plt.plot(c) +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 290cd566..57957331 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -199,7 +199,7 @@ def state_estimation(): # Set up track parameters l = Localization() l.create_track() - l.prepare_trajectory(0.06) + #l.prepare_trajectory(0.06) d_f_hist = [0.0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag d_f_lp = 0.0 From 70923be6f6d8585aa27e39d3e8575e48da677185 Mon Sep 17 00:00:00 2001 From: Ugo Date: Mon, 5 Dec 2016 22:41:02 -0800 Subject: [PATCH 101/183] Good tuning. --- env_loader_pc.sh | 4 ++-- eval_sim/eval_data.jl | 10 ++++++---- odroid_setup.sh | 2 ++ workspace/src/barc/launch/open_loop_remote.launch | 2 +- workspace/src/barc/launch/run_lmpc_remote.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 2 +- workspace/src/barc/src/Localization_helpers.py | 4 ++-- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/controller_low_level.py | 5 ++--- 9 files changed, 18 insertions(+), 15 deletions(-) create mode 100644 odroid_setup.sh diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 0c38405f..031bf10e 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -2,6 +2,6 @@ source /opt/ros/kinetic/setup.bash source /home/max/barc/workspace/devel/setup.bash export PATH=$PATH:/home/max/Downloads/julia-2e358ce975/bin -export ROS_IP=192.168.100.152 -export ROS_MASTER_URI=http://192.168.100.152:11311 +export ROS_IP=192.168.100.72 +export ROS_MASTER_URI=http://192.168.100.72:11311 exec "$@" \ No newline at end of file diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 14e1c68b..94016e2b 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -2,7 +2,7 @@ using JLD using PyPlot using PyCall @pyimport matplotlib.animation as animation -using HDF5, JLD, ProfileView +using JLD, ProfileView # pos_info[1] = s # pos_info[2] = eY # pos_info[3] = ePsi @@ -1023,11 +1023,13 @@ function create_track(w) # SIMPLE GOGGLE TRACK add_curve(theta,30,0) add_curve(theta,40,-pi/2) + add_curve(theta,10,0) add_curve(theta,40,-pi/2) - add_curve(theta,20,-pi/10) - add_curve(theta,30,pi/5) - add_curve(theta,20,-pi/10) + add_curve(theta,20,pi/10) + add_curve(theta,30,-pi/5) + add_curve(theta,20,pi/10) add_curve(theta,40,-pi/2) + add_curve(theta,10,0) add_curve(theta,40,-pi/2) add_curve(theta,35,0) diff --git a/odroid_setup.sh b/odroid_setup.sh new file mode 100644 index 00000000..5311301d --- /dev/null +++ b/odroid_setup.sh @@ -0,0 +1,2 @@ +export ROS_MASTER_URI=http://192.168.100.72:11311 +export ROS_IP=192.168.100.72 \ No newline at end of file diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index f292d232..11ca8aa9 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -8,7 +8,7 @@ - + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 3d0e2ea9..b0b042a8 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -8,7 +8,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 70feafc1..e6450168 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -112,7 +112,7 @@ function main() log_t = zeros(10000,1) log_state = zeros(10000,7) log_cost = zeros(10000,6) - log_c_Vx = zeros(10000,4) + log_c_Vx = zeros(10000,3) log_c_Vy = zeros(10000,4) log_c_Psi = zeros(10000,3) log_cmd = zeros(10000,2) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 1b207231..f9d094a6 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -383,8 +383,8 @@ def find_s(self): epsi = (self.psi+pi)%(2*pi)-pi-xyPathAngle epsi = (epsi+pi)%(2*pi)-pi - if s < 0.0: - s = s + self.n*self.ds + #if s < 0.0: + # s = s + self.n*self.ds self.epsi = epsi self.ey = ey diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 26a0ca75..37427ee7 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 26a0ca7591f8516d4198a54e2074434f28425b2e +Subproject commit 37427ee7c82da8c8b427fda088251216a9b0caa6 diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 789c5386..2b8e2107 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -49,9 +49,8 @@ def pwm_converter_callback(self, msg): if FxR == 0: self.motor_pwm = 90.0 elif FxR > 0: - self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino - #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino - #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down self.motor_pwm = 93.5 + 46.73*FxR From 9d9105ade4f821bc3d73b4bb0d9a56b01f87b06a Mon Sep 17 00:00:00 2001 From: Ugo Date: Tue, 6 Dec 2016 10:15:54 -0800 Subject: [PATCH 102/183] Updated localization parameters --- workspace/src/barc/src/Localization_helpers.py | 8 ++++---- workspace/src/barc/src/barc_lib | 2 +- workspace/src/barc/src/debug_localization.py | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index f9d094a6..c3f93f1a 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -39,12 +39,12 @@ class Localization(object): pos = 0 # current position psi = 0 # current orientation nodes = array([0]) # all nodes are saved in a matrix - N_nodes_poly_back = 20 # number of nodes behind current position - N_nodes_poly_front = 50 # number of nodes in front + N_nodes_poly_back = 30 # number of nodes behind current position + N_nodes_poly_front = 80 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 4 # order of x-y-polynomial interpolation - OrderThetaCurv = 3 # order of theta interpolation + OrderXY = 8 # order of x-y-polynomial interpolation + OrderThetaCurv = 6 # order of theta interpolation closed = True # open or closed trajectory? coeffCurvature = zeros(4) diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index 37427ee7..c190be68 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit 37427ee7c82da8c8b427fda088251216a9b0caa6 +Subproject commit c190be681db3a2d68737a7bb149ef500807ba0a4 diff --git a/workspace/src/barc/src/debug_localization.py b/workspace/src/barc/src/debug_localization.py index 76fb72d3..148b14cb 100644 --- a/workspace/src/barc/src/debug_localization.py +++ b/workspace/src/barc/src/debug_localization.py @@ -19,7 +19,7 @@ for i in range(0,n): l.set_pos(l.nodes[0,i], l.nodes[1,i], 0.0,0.0,0.0,0.0) l.find_s() - s = linspace(l.s,l.s+1.0,5) + s = linspace(l.s,l.s+2.0,5) ss[i] = l.s #c[i,:] = polyval(l.coeffCurvature,s) c = polyval(l.coeffCurvature,s) From 0614cedbd5b53b447efdd2b70c6f5dccb646aba1 Mon Sep 17 00:00:00 2001 From: Ugo Date: Tue, 6 Dec 2016 15:18:47 -0800 Subject: [PATCH 103/183] Updated localization parameters, increased oldTrajectory buffer for higher speeds --- eval_sim/eval_data.jl | 6 ++++-- workspace/src/barc/src/LMPC_node.jl | 4 ++-- workspace/src/barc/src/Localization_helpers.py | 6 +++--- workspace/src/barc/src/barc_lib | 2 +- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 94016e2b..3a69822d 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -318,7 +318,8 @@ function eval_LMPC(code::AbstractString) for i=1:size(curv,1) s = state[i,6] #c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] - c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + c[i] = ([s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] end plot(state[:,6],c,"-o") for i=1:5:size(curv,1) @@ -329,7 +330,8 @@ function eval_LMPC(code::AbstractString) end c = zeros(size(curv,1),1) #c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + c = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]' plot(s,c,"-*") end title("Curvature over path") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index e6450168..67cf3a3d 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -47,7 +47,7 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po oldTraj.count[lapStatus.currentLap] += 1 # if necessary: append to end of previous lap - if lapStatus.currentLap > 1 && z_est[6] < 12.0 + if lapStatus.currentLap > 1 && z_est[6] < 16.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] @@ -57,7 +57,7 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po end #if necessary: append to beginning of next lap - if z_est[6] > posInfo.s_target - 12.0 + if z_est[6] > posInfo.s_target - 16.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index c3f93f1a..acdee553 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -40,11 +40,11 @@ class Localization(object): psi = 0 # current orientation nodes = array([0]) # all nodes are saved in a matrix N_nodes_poly_back = 30 # number of nodes behind current position - N_nodes_poly_front = 80 # number of nodes in front + N_nodes_poly_front = 100 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total - OrderXY = 8 # order of x-y-polynomial interpolation - OrderThetaCurv = 6 # order of theta interpolation + OrderXY = 10 # order of x-y-polynomial interpolation + OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? coeffCurvature = zeros(4) diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib index c190be68..8e266c57 160000 --- a/workspace/src/barc/src/barc_lib +++ b/workspace/src/barc/src/barc_lib @@ -1 +1 @@ -Subproject commit c190be681db3a2d68737a7bb149ef500807ba0a4 +Subproject commit 8e266c57a4116b51e0a23339c33cd0d5473e3cb1 From ffe35b1b66b24e5ceab31b64565bcd04d85e4843 Mon Sep 17 00:00:00 2001 From: Maximilian Brunner Date: Wed, 7 Dec 2016 08:19:46 -0800 Subject: [PATCH 104/183] Added evaluation functions for thesis. --- eval_sim/eval_data.jl | 60 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 14e1c68b..31cbcda9 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -192,6 +192,66 @@ function eval_run(code::AbstractString) nothing end +function plot_v_ey_over_s(code::AbstractString,laps::Array{Int64}) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + n_laps = size(laps,1) + + println("OldCost: ",oldTraj.oldCost) + # plot v_x over s + figure(1) + for i=1:n_laps + idx = (oldTraj.oldTraj[:,6,laps[i]] .>= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) + plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,1,laps[i]],label="Lap $(laps[i])") + end + legend() + xlabel("s [m]") + ylabel("v [m/s]") + + # plot e_y over s + figure(2) + for i=1:n_laps + idx = (oldTraj.oldTraj[:,6,laps[i]] .>= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) + plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,5,laps[i]],label="Lap $(laps[i])") + end + legend() + xlabel("s [m]") + ylabel("e_Y [m]") +end + +function plot_v_over_xy(code::AbstractString,lap::Int64) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + + # Find timing of selected lap: + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end] + println("Laptime = $(t_end-t_start) s") + + idx = (pos_info.t.>=t_start) & (pos_info.t.<=t_end) + + track = create_track(0.4) + + figure() + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face") + grid(1) + title("x-y-view") + axis("equal") + cb = colorbar() + cb[:set_label]("Velocity [m/s]") + println("Average v_x = ",mean(pos_info.z[idx,8])," m/s") + +end + + function eval_open_loop(code::AbstractString) log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" d_rec = load(log_path_record) From e5283d859eff57c4cc9763568c7ac26171c1fcd5 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Thu, 8 Dec 2016 09:37:31 -0800 Subject: [PATCH 105/183] Added evaluation functions. --- eval_sim/eval_data.jl | 66 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 62 insertions(+), 4 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index eebf3464..95242edb 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -129,13 +129,16 @@ function eval_run(code::AbstractString) legend(["GPS meas","estimate"]) figure() - plot(imu_meas.t-t0,imu_meas.z[:,7:9]) + #plot(imu_meas.t-t0,imu_meas.z[:,7:9]) plot(pos_info.t-t0,pos_info.z[:,19:20]) grid("on") + legend(["a_x","a_y"]) + ylabel("a [m/s^2]") + xlabel("t [s]") title("Measured accelerations") figure() - plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-x",gps_meas.t_msg-t0,gps_meas.z,"--x") + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-",gps_meas.t_msg-t0,gps_meas.z,"--") grid(1) title("GPS comparison") xlabel("t [s]") @@ -192,6 +195,27 @@ function eval_run(code::AbstractString) nothing end +function plot_friction_circle(code::AbstractString,lap::Int64) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + + # Find timing of selected lap: + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end] + idx = (pos_info.t.>t_start) & (pos_info.t.= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) - plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,1,laps[i]],label="Lap $(laps[i])") + plot(smooth(oldTraj.oldTraj[idx,6,laps[i]],5),oldTraj.oldTraj[idx,1,laps[i]],label="Lap $(laps[i])") end legend() + grid("on") xlabel("s [m]") ylabel("v [m/s]") + xlim([0,19.11]) + ylim([0,3.2]) # plot e_y over s figure(2) @@ -217,8 +244,31 @@ function plot_v_ey_over_s(code::AbstractString,laps::Array{Int64}) plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,5,laps[i]],label="Lap $(laps[i])") end legend() + grid("on") xlabel("s [m]") ylabel("e_Y [m]") + xlim([0,19.11]) + ylim([-0.5,0.5]) + + # plot lap time over iterations + laps_run = size(oldTraj.oldCost[oldTraj.oldCost.>1],1) + lap_times = zeros(laps_run) + for i=1:laps_run + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].>=0,i][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].<=19.11,i][end] + lap_times[i] = t_end-t_start + end + figure(3) + plot(1:size(lap_times,1),lap_times,"-o") + grid("on") + xlabel("Lap number") + ylabel("Lap time [s]") + ylim([0,25]) +end + +function plots_for_presentation() + code = "dc15" + laps = [2,4,6,17,18] end function plot_v_over_xy(code::AbstractString,lap::Int64) @@ -241,7 +291,7 @@ function plot_v_over_xy(code::AbstractString,lap::Int64) figure() plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face") + scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=0.5,vmax=2.0) grid(1) title("x-y-view") axis("equal") @@ -1172,6 +1222,7 @@ function checkTimes(code::AbstractString) cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC sol_status_int = zeros(size(t_solv,1)) t = d_lmpc["t"] + t0 = t[1] for i=1:size(sol_status,1) if sol_status[i]==:Optimal @@ -1189,6 +1240,13 @@ function checkTimes(code::AbstractString) subplot(212,sharex=ax1) plot(t,cmd,"-x") grid("on") + + figure(2) + plot(t-t0,t_solv) + title("Solver time") + grid("on") + xlabel("t [s]") + ylabel("Solver time [s]") end function checkConnectivity(code::AbstractString) From 63f74f7227d7d5b7502885f5e73aa8ab6b74e51a Mon Sep 17 00:00:00 2001 From: maxb91 Date: Thu, 8 Dec 2016 15:58:02 -0800 Subject: [PATCH 106/183] Deleted submodule, replaced it with normal directory --- .gitmodules | 3 - workspace/src/barc/src/barc_lib | 1 - .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 370 ++++++++++++++++++ .../src/barc_lib/LMPC/coeffConstraintCost.jl | 252 ++++++++++++ .../src/barc/src/barc_lib/LMPC/functions.jl | 112 ++++++ .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 112 ++++++ workspace/src/barc/src/barc_lib/README.md | 5 + workspace/src/barc/src/barc_lib/classes.jl | 95 +++++ .../src/barc/src/barc_lib/log_functions.jl | 75 ++++ workspace/src/barc/src/barc_lib/simModel.jl | 138 +++++++ 10 files changed, 1159 insertions(+), 4 deletions(-) delete mode 160000 workspace/src/barc/src/barc_lib create mode 100644 workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl create mode 100644 workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl create mode 100644 workspace/src/barc/src/barc_lib/LMPC/functions.jl create mode 100644 workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl create mode 100644 workspace/src/barc/src/barc_lib/README.md create mode 100644 workspace/src/barc/src/barc_lib/classes.jl create mode 100644 workspace/src/barc/src/barc_lib/log_functions.jl create mode 100644 workspace/src/barc/src/barc_lib/simModel.jl diff --git a/.gitmodules b/.gitmodules index 8761a04b..e69de29b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "workspace/src/barc/src/barc_lib"] - path = workspace/src/barc/src/barc_lib - url = https://github.com/maxb91/barc_lib diff --git a/workspace/src/barc/src/barc_lib b/workspace/src/barc/src/barc_lib deleted file mode 160000 index 8e266c57..00000000 --- a/workspace/src/barc/src/barc_lib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8e266c57a4116b51e0a23339c33cd0d5473e3cb1 diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl new file mode 100644 index 00000000..4e4438fb --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -0,0 +1,370 @@ +type MpcModel + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,1} + c_Vy::Array{JuMP.NonlinearParameter,1} + c_Psi::Array{JuMP.NonlinearParameter,1} + coeffTermConst::Array{JuMP.NonlinearParameter,3} + coeffTermCost::Array{JuMP.NonlinearParameter,2} + + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + ParInt::JuMP.Variable + + laneCost::JuMP.NonlinearExpression + constZTerm::JuMP.NonlinearExpression + costZTerm::JuMP.NonlinearExpression + derivCost::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + costZ::JuMP.NonlinearExpression + + uPrev::Array{JuMP.NonlinearParameter,2} + + function MpcModel(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff) + m = new() + dt = modelParams.dt + L_a = modelParams.l_A + L_b = modelParams.l_B + c0 = modelParams.c0 + u_lb = modelParams.u_lb + u_ub = modelParams.u_ub + z_lb = modelParams.z_lb + z_ub = modelParams.z_ub + + N = mpcParams.N + Q = mpcParams.Q + Q_term = mpcParams.Q_term + R = mpcParams.R + order = mpcCoeff.order # polynomial order of terminal constraints and cost approximation + ey_max = trackCoeff.width/2 + + QderivZ = mpcParams.QderivZ::Array{Float64,1} + QderivU = mpcParams.QderivU::Array{Float64,1} + Q_term_cost = mpcParams.Q_term_cost::Float64 + delay_df = mpcParams.delay_df + delay_a = mpcParams.delay_a + + acc_f = 1.0 + + n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation + + # Path following mode: + # Create function-specific parameters + v_ref = mpcParams.vPathFollowing + z_Ref::Array{Float64,2} + z_Ref = cat(2,v_ref*ones(N+1,1),zeros(N+1,5)) # Reference trajectory: path following -> stay on line and keep constant velocity + u_Ref = zeros(N,2) + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.08))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @variable( mdl, z_Ol[1:(N+1),1:7]) + @variable( mdl, u_Ol[1:N,1:2]) + @variable( mdl, 0 <= ParInt <= 1) + #@variable( mdl, eps[1:2] >= 0) # eps for soft lane constraints + @variable( mdl, eps[1:N+1] >= 0) # eps for soft lane constraints + + z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states + z_ub_6s = ones(mpcParams.N+1,1)*[2.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_6s[j,i]) + setupperbound(u_Ol[j,i], u_ub_6s[j,i]) + end + end + for i=1:7 + for j=1:N+1 + setlowerbound(z_Ol[j,i], z_lb_6s[j,i]) + setupperbound(z_Ol[j,i], z_ub_6s[j,i]) + end + end + + @NLparameter(mdl, z0[i=1:7] == 0) + @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0) + @NLparameter(mdl, c_Vx[i=1:3] == 0) + @NLparameter(mdl, c_Vy[i=1:4] == 0) + @NLparameter(mdl, c_Psi[i=1:3] == 0) + @NLparameter(mdl, coeffTermConst[i=1:order+1,j=1:2,k=1:5] == 0) + @NLparameter(mdl, coeffTermCost[i=1:order+1,j=1:2] == 0) + @NLparameter(mdl, uPrev[1:10,1:2] == 0) + + # Conditions for first solve: + setvalue(z0[1],1) + setvalue(c_Vx[3],0.1) + + @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i]) + #@NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] <= ey_max + eps[1]) + #@NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] >= -ey_max - eps[2]) + @NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] <= ey_max + eps[i]) + @NLconstraint(mdl, [i=1:N+1], z_Ol[i,5] >= -ey_max - eps[i]) + + @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) + @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i])) + + println("Initializing model...") + + # System dynamics + for i=1:N + if i<=delay_df + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2]) # psiDot + else + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2]) # psiDot + end + if i<=delay_a + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1])) + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f) + else + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f) + end + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7]) # xDot + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1])) # xDot + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7]) + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i])) # ePsi + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY + @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s + end + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end + + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + end + + # Cost functions + + # Derivative cost + # --------------------------------- + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} + + QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ + QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) + + # Lane cost + # --------------------------------- + #@NLexpression(mdl, laneCost, sum{100000*eps[i]+1000*eps[i]^2,i=1:2}) + @NLexpression(mdl, laneCost, sum{10*eps[i]+100*eps[i]^2,i=2:N+1}) + + # Lane cost + # --------------------------------- + #@NLexpression(mdl, laneCost, 100*sum{z_Ol[i,5]^2*((0.5+0.5*tanh(10*(z_Ol[i,5]-ey_max))) + (0.5-0.5*tanh(10*(z_Ol[i,5]+ey_max)))),i=1:N+1}) + + # Control Input cost + # --------------------------------- + @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+ + R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df}) + + # Terminal constraints (soft), starting from 2nd lap + # --------------------------------- + @NLexpression(mdl, constZTerm, sum{Q_term[j]*(ParInt*(sum{coeffTermConst[i,1,j]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermConst[order+1,1,j])+ + (1-ParInt)*(sum{coeffTermConst[i,2,j]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermConst[order+1,2,j])-z_Ol[N+1,j])^2,j=1:5}) + + # Terminal cost + # --------------------------------- + # The value of this cost determines how fast the algorithm learns. The higher this cost, the faster the control tries to reach the finish line. + @NLexpression(mdl, costZTerm, 1/5*(Q_term_cost*(ParInt*(sum{coeffTermCost[i,1]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermCost[order+1,1])+ + (1-ParInt)*(sum{coeffTermCost[i,2]*z_Ol[N+1,6]^(order+1-i),i=1:order}+coeffTermCost[order+1,2])))) + + # State cost (only for path following mode) + # --------------------------------- + #@NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=1:N+1},i=1:6}) # Follow trajectory + #@NLexpression(mdl, costZ, 0.5*sum{(Q[1]*(sqrt(z_Ol[j,1]^2+z_Ol[j,2]^2)-1.0)^2 + Q[4]*z_Ol[j,4]^2 + Q[5]*z_Ol[j,5]^2),j=2:N+1}) + + @NLexpression(mdl, costZ, 0*QderivU[1]*sum{z_Ol[i,7],i=1:N}) + # Solve model once + @NLobjective(mdl, Min, derivCost + constZTerm + costZTerm + laneCost + N*Q_term_cost) + #@NLobjective(mdl, Min, derivCost + costZ) + sol_stat=solve(mdl) + println("Finished solve 1: $sol_stat") + sol_stat=solve(mdl) + println("Finished solve 2: $sol_stat") + m.mdl = mdl + m.z0 = z0 + m.coeff = coeff + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.ParInt = ParInt + m.uPrev = uPrev + + m.coeffTermCost = coeffTermCost + m.coeffTermConst = coeffTermConst + + m.derivCost = derivCost + m.controlCost = controlCost + m.laneCost = laneCost + m.constZTerm = constZTerm + m.costZTerm = costZTerm + m.costZ = costZ + return m + end +end + +type MpcModel_pF + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + derivCost::JuMP.NonlinearExpression + costZ::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + + uPrev::Array{JuMP.NonlinearParameter,2} + + function MpcModel_pF(mpcParams::MpcParams,modelParams::ModelParams,trackCoeff::TrackCoeff) + println("Starting creation of pf model") + m = new() + dt = modelParams.dt + L_a = modelParams.l_A + L_b = modelParams.l_B + c0 = modelParams.c0 + u_lb = modelParams.u_lb + u_ub = modelParams.u_ub + z_lb = modelParams.z_lb + z_ub = modelParams.z_ub + + N = mpcParams.N + Q = mpcParams.Q + R = mpcParams.R + QderivZ = mpcParams.QderivZ::Array{Float64,1} + QderivU = mpcParams.QderivU::Array{Float64,1} + delay_df = mpcParams.delay_df::Int64 + delay_a = mpcParams.delay_a::Int64 + + v_ref = mpcParams.vPathFollowing + + acc_f = 1.0 + + n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation + + # Create function-specific parameters + z_Ref::Array{Float64,2} + z_Ref = cat(2,zeros(N+1,3),v_ref*ones(N+1,1)) # Reference trajectory: path following -> stay on line and keep constant velocity + u_Ref = zeros(N,2) + + # Create Model + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.07))#,linear_solver="ma57",print_user_options="yes")) + + # Create variables (these are going to be optimized) + @variable( mdl, z_Ol[1:(N+1),1:5], start = 0) # z = s, ey, epsi, v + @variable( mdl, u_Ol[1:N,1:2], start = 0) + + # Set bounds + z_lb_4s = ones(mpcParams.N+1,1)*[-Inf -Inf -Inf -0.5] # lower bounds on states + z_ub_4s = ones(mpcParams.N+1,1)*[ Inf Inf Inf 1.5] # upper bounds + u_lb_4s = ones(mpcParams.N,1) * [0.0 -0.3] # lower bounds on steering + u_ub_4s = ones(mpcParams.N,1) * [1.2 0.3] # upper bounds + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_4s[j,i]) + setupperbound(u_Ol[j,i], u_ub_4s[j,i]) + end + end + # for i=1:4 + # for j=1:N+1 + # setlowerbound(z_Ol[j,i], z_lb_4s[j,i]) + # setupperbound(z_Ol[j,i], z_ub_4s[j,i]) + # end + # end + + @NLparameter(mdl, z0[i=1:5] == 0) + @NLparameter(mdl, uPrev[1:10,1:2] == 0) + + @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0) + + @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,1]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) + + # System dynamics + setvalue(z0[4],v_ref) + @NLconstraint(mdl, [i=1:5], z_Ol[1,i] == z0[i]) # initial condition + for i=1:N + if i<=delay_df + @NLexpression(mdl, bta[i], atan( L_a / (L_a + L_b) * tan( uPrev[delay_df+1-i,2] ) ) ) + else + @NLexpression(mdl, bta[i], atan( L_a / (L_a + L_b) * tan( u_Ol[i-delay_df,2] ) ) ) + end + if i<=delay_a + #@NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,4])) # v + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(uPrev[delay_a+1-i,1] - z_Ol[i,5])*acc_f) # v + else + #@NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(u_Ol[i-delay_a,1] - 0.5*z_Ol[i,4])) # v + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(u_Ol[i-delay_a,1] - z_Ol[i,5])*acc_f) # v + end + + @NLexpression(mdl, dsdt[i], z_Ol[i,4]*cos(z_Ol[i,3]+bta[i])/(1-z_Ol[i,2]*c[i])) + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*dsdt[i] ) # s + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + dt*z_Ol[i,4]*sin(z_Ol[i,3]+bta[i]) ) # ey + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + dt*(z_Ol[i,4]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) # epsi + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,5] - 0.5*z_Ol[i,4])) # v + end + + # Cost definitions + # Derivative cost + # --------------------------------- + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:4} + + QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ + QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) + + # Control Input cost + # --------------------------------- + @NLexpression(mdl, controlCost, 0.5*R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+ + 0.5*R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df}) + + # State cost + # --------------------------------- + @NLexpression(mdl, costZ, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:4}) # Follow trajectory + + # Objective function + @NLobjective(mdl, Min, costZ + derivCost + controlCost) + + # create first artificial solution (for warm start) + for i=1:N+1 + setvalue(z_Ol[i,:],[(i-1)*dt*v_ref 0 0 v_ref 0]) + end + for i=1:N + setvalue(u_Ol[i,:],[0.15 0]) + end + # First solve + sol_stat=solve(mdl) + println("Finished solve 1: $sol_stat") + sol_stat=solve(mdl) + println("Finished solve 2: $sol_stat") + + m.mdl = mdl + m.z0 = z0 + m.coeff = coeff + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.uPrev = uPrev + m.derivCost = derivCost + m.costZ = costZ + m.controlCost = controlCost + return m + end +end + +# why does the solution change when I start the simulation twice, totally independently? Is not everything initialized equally? +# why do I get a failed restoration phase even though there are *almost* no constraints and the solution should be super clear? + +# does it actually make sense to use the first (initial) state as a @variable and combine it with a constraint to fix it to z0 ? +# or better use only a parameter for z0? -> Found out that solutions are different if I set constraints for first state or not!? diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl new file mode 100644 index 00000000..54e396a7 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -0,0 +1,252 @@ +# This function evaluates and returns the coefficients for constraints and cost which are used later in the MPC formulation +# Inputs are: +# oldTraj -> contains information about previous trajectories and Inputs +# mpcCoeff -> contains information about previous coefficient results +# posInfo -> contains information about the car's current position along the track +# mpcParams -> contains information about the MPC formulation (e.g. Q, R) +# stateIn -> current state of the car +# inputIn -> last input to the system + +# structure of oldTrajectory: 1st dimension = state number, 2nd dimension = step number (time equiv.), 3rd dimennsion = lap number + +# z[1] = xDot +# z[2] = yDot +# z[3] = psiDot +# z[4] = ePsi +# z[5] = eY +# z[6] = s + +function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams,lapStatus::LapStatus) + # this computes the coefficients for the cost and constraints + + # Outputs: + # coeffConst + # coeffCost + + # Read Inputs + s = posInfo.s + s_target = posInfo.s_target + + # Parameters + Order = mpcCoeff.order # interpolation order for cost and constraints + pLength = mpcCoeff.pLength # interpolation length for polynomials + delay_df = mpcParams.delay_df + #delay = 2 + + #coeffCost = zeros(Order+1,2) # polynomial coefficients for cost + #coeffConst = zeros(Order+1,2,5) # nz-1 beacuse no coeff for s + + n_laps_sysID = 2 # number of previous laps that are used for sysID + + selected_laps = zeros(Int64,2) + selected_laps[1] = lapStatus.currentLap-1 # use previous lap + selected_laps[2] = lapStatus.currentLap-2 # and the one before + if lapStatus.currentLap >= 5 + selected_laps[2] = indmin(oldTraj.oldCost[2:lapStatus.currentLap-2])+1 # and the best from all previous laps + end + + # Select the old data + oldxDot = oldTraj.oldTraj[:,1,selected_laps]::Array{Float64,3} + oldyDot = oldTraj.oldTraj[:,2,selected_laps]::Array{Float64,3} + oldpsiDot = oldTraj.oldTraj[:,3,selected_laps]::Array{Float64,3} + oldePsi = oldTraj.oldTraj[:,4,selected_laps]::Array{Float64,3} + oldeY = oldTraj.oldTraj[:,5,selected_laps]::Array{Float64,3} + oldS = oldTraj.oldTraj[:,6,selected_laps]::Array{Float64,3} + oldacc = oldTraj.oldTraj[:,7,selected_laps]::Array{Float64,3} + olda = oldTraj.oldInput[:,1,selected_laps]::Array{Float64,3} + olddF = oldTraj.oldInput[:,2,selected_laps]::Array{Float64,3} + #olddF = smooth(olddF,5) + + N_points = size(oldTraj.oldTraj,1) # second dimension = length (=buffersize) + + s_total::Float64 # initialize + DistS::Array{Float64} # initialize + idx_s::Array{Int64} # initialize + idx_s_target = 0 + dist_to_s_target = 0 + qLength = 0 + vec_range::Tuple{UnitRange{Int64},UnitRange{Int64}} + bS_Vector::Array{Float64} + s_forinterpy::Array{Float64} + + # Compute the total s (current position along track) + s_total = s % s_target + + # Compute the index + DistS = ( s_total - oldS ).^2 + + idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! + + vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) + + # Create the vectors used for the interpolation + # bS_vector contains the s-values for later interpolation + bS_Vector = zeros(pLength+1,2) + for i=1:pLength+1 + bS_Vector[i,1] = oldS[vec_range[1][i]] + bS_Vector[i,2] = oldS[vec_range[2][i]] + end + # if norm(bS_Vector[1,1]-s_total) > 0.3 || norm(bS_Vector[1,2]-s_total) > 0.3 + # warn("Couldn't find a close point to current s.") + # end + + # The states are parametrized with resprect to the curvilinear abscissa, + # so we select the point used for the interpolation. Need to subtract an + # offset to be coherent with the MPC formulation + s_forinterpy = bS_Vector + if s_total < 0 + s_forinterpy += s_target + end + + # Create the Matrices for the interpolation + MatrixInterp = zeros(pLength+1,Order+1,2) + + for k = 0:Order + MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k + end + + # Compute the coefficients + #coeffConst = zeros(Order+1,2,5) + for i=1:2 + mpcCoeff.coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldxDot[vec_range[i]] + mpcCoeff.coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldyDot[vec_range[i]] + mpcCoeff.coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldpsiDot[vec_range[i]] + mpcCoeff.coeffConst[:,i,4] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] + mpcCoeff.coeffConst[:,i,5] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] + end + + # Finished with calculating the constraint coefficients + + # Now compute the final cost coefficients + + # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value + # These values are calculated for both old trajectories + # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line + # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost + for i=1:2 + dist_to_s_target = oldTraj.oldCost[selected_laps[i]] + oldTraj.idx_start[selected_laps[i]] - (idx_s[i]-N_points*(i-1)) # number of iterations from idx_s to s_target + bQfunction_Vector = collect(linspace(dist_to_s_target,dist_to_s_target-pLength,pLength+1)) # build a vector that starts at the distance and + # decreases in equal steps + mpcCoeff.coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector # interpolate this vector with the given s + end + + # --------------- SYSTEM IDENTIFICATION --------------- # + # ----------------------------------------------------- # + + cC = oldTraj.count[lapStatus.currentLap]-1 # current count + cL = lapStatus.currentLap # current lap + #vec_range_ID2 = cC-n_prev:cC-1 # index range for current lap + + # TODO:******************* + # CHECK IF DIFFERENCES ARE ALIGNED PROPERLY + # ADD 2-step-delay to sysID + # ************************ + # collect indices for system ID + n_sys_ID_prev = 60 # steps of sysID before current point in previous laps + n_sys_ID_post = 60 # steps of sysID after current point in previous laps + n_sys_ID_prev_c = 30 # steps of sysID before current point in current lap + + # vec_range_ID = () + # for i=1:n_laps_sysID + # vec_range_ID = tuple(vec_range_ID...,idx_s[i]-n_prev:idx_s[i]+n_ahead) # related index range + # end + + sysID_idx_diff = idx_s[1]-n_sys_ID_prev*5-1 + (1:5:(n_sys_ID_prev+n_sys_ID_post+1)*5) # these are the indices that are used for differences + sysID_idx = sysID_idx_diff[1:end-1] + + sysID_idx_diff2 = idx_s[2]-n_sys_ID_prev*5-1 + (1:5:(n_sys_ID_prev+n_sys_ID_post+1)*5) # these are the indices that are used for differences + sysID_idx2 = sysID_idx_diff2[1:end-1] + + sysID_idx_diff_c = cC - (n_sys_ID_prev_c+1)*5-1 + (1:5:(n_sys_ID_prev_c+1)*5) + sysID_idx_c = sysID_idx_diff_c[1:end-1] + + sz1 = size(sysID_idx,1) + sz2 = size(sysID_idx_c,1) + sz2 = 0 + sz = sz1 + sz2 + + # psiDot + y_psi = zeros((2*sz1+sz2)*5) + A_psi = zeros((2*sz1+sz2)*5,3) + for i=0:4 + y_psi[(1:sz1)+i*sz1] = diff(oldpsiDot[sysID_idx_diff+i]) + A_psi[(1:sz1)+i*sz1,:] = [oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i]] + #y_psi[(1:sz2)+i*sz2+5*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) + #A_psi[(1:sz2)+i*sz2+5*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*5,2,cL]] + y_psi[(1:sz1)+i*sz1+5*sz1+5*sz2] = diff(oldpsiDot[sysID_idx_diff2+i]) + A_psi[(1:sz1)+i*sz1+5*sz1+5*sz2,:] = [oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i]] + end + + # xDot + y_xDot = zeros((2*sz1+sz2)*5) + A_xDot = zeros((2*sz1+sz2)*5,3) + for i=0:4 + y_xDot[(1:sz1)+i*sz1] = diff(oldxDot[sysID_idx_diff+i]) + A_xDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i].*oldpsiDot[sysID_idx+i] oldxDot[sysID_idx+i] oldacc[sysID_idx+i]] + #y_xDot[(1:sz2)+i*sz2+5*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) + #A_xDot[(1:sz2)+i*sz2+5*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] + y_xDot[(1:sz1)+i*sz1+5*(sz1+sz2)] = diff(oldxDot[sysID_idx_diff2+i]) + A_xDot[(1:sz1)+i*sz1+5*(sz1+sz2),:] = [oldyDot[sysID_idx2+i].*oldpsiDot[sysID_idx2+i] oldxDot[sysID_idx2+i] oldacc[sysID_idx2+i]] + end + + # yDot + y_yDot = zeros((2*sz1+sz2)*5) + A_yDot = zeros((2*sz1+sz2)*5,4) + for i=0:4 + y_yDot[(1:sz1)+i*sz1] = diff(oldyDot[sysID_idx_diff+i]) + A_yDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i].*oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i-delay_df*5]] + #y_yDot[(1:sz2)+i*sz2+5*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) + #A_yDot[(1:sz2)+i*sz2+5*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*5,2]] + y_yDot[(1:sz1)+i*sz1+5*(sz1+sz2)] = diff(oldyDot[sysID_idx_diff2+i]) + A_yDot[(1:sz1)+i*sz1+5*(sz1+sz2),:] = [oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i].*oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i-delay_df*5]] + end + + # if any(isnan,y_yDot) # check if any value in the y_yDot value is NaN + # println(y_yDot) + # warn("NaN value detected in y_yDot! Press to continue...") + # end + # if any(isnan,coeffCost) + # println(coeffCost) + # warn("NaN value detected in coeffCost! Press to continue...") + # end + # if any(isnan,coeffConst) + # println(coeffCost) + # warn("NaN value detected in coeffConst! Press to continue...") + # end + + # mpcCoeff.c_Psi = zeros(3) + # mpcCoeff.c_Vx = zeros(4) + # mpcCoeff.c_Vy = zeros(4) + + mpcCoeff.c_Psi = (A_psi'*A_psi)\A_psi'*y_psi + mpcCoeff.c_Vx = (A_xDot'*A_xDot)\A_xDot'*y_xDot # the identity matrix is used to scale the coefficients + mpcCoeff.c_Vy = (A_yDot'*A_yDot)\A_yDot'*y_yDot + + # if det(A_psi'*A_psi) != 0 + # else + # println("det y_psi = 0") + # end + # if det(A_xDot'*A_xDot) != 0 + # else + # println("det vx = 0") + # end + # if det(A_yDot'*A_yDot) != 0 + # else + # println("det vy = 0") + # end + #println("Done. c_Psi = $(mpcCoeff.c_Psi)") + #mpcCoeff.c_Psi = [-1.0,-4.5,5.8] + #mpcCoeff.c_Vy = [-1.2,-0.04,-0.01,0.6] + #mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724] + #mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765] + #mpcCoeff.coeffCost = coeffCost + #mpcCoeff.coeffConst = coeffConst + nothing +end + +# Notes about oldTrajectory: +# oldTrajectory[1:prebuf] = states before finish line (s < 0) +# oldTrajectory[prebuf+1:prebuf+cost] = states between start and finish line (0 <= s < s_target) +# oldTrajectory[prebuf+cost+1:prebuf+cost+postbuf] = states after finish line (s_target <= s) +# once one lap is over, the states are saved (interval prebuf:s_target) +# during the next lap, the states behind the finish line are appended (interval s_target:postbuf) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl new file mode 100644 index 00000000..a6b4a379 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -0,0 +1,112 @@ +# Important information about oldTraj: +# ====================================== +# oldTraj.oldTraj contains all state information of one lap. It is structured like follows: +# The first values are the end of one lap before the next lap starts (necessary for inter-lap-system-ID) +# The last value of ends with the last value *before* the finish line (s < s_target) +# The s-values within need to be below zero (-> before the finish line). Otherwise they can be mistaken as the end of the current (not previous) lap. +# After follow the recorded states of the trajectory (as many as there were during one lap) +# The number of recorded states during one trajectory (0 <= s < s_target) is equal to . +# After the recorded trajectory, the rest of the vector (until ) is filled up with constant values + +function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,buffersize::Int64) + println("Starting function") + i = lapStatus.currentIt-1 # i = number of points for 0 <= s < s_target (= cost of this lap) + prebuf = oldTraj.prebuf # so many points of the end of the previous old traj will be attached to the beginning + zCurr_export = zeros(buffersize,6) + uCurr_export = zeros(buffersize,2) + + println("Creating exports") + zCurr_export = cat(1,oldTraj.oldTraj[oldTraj.oldCost[1]+1:oldTraj.oldCost[1]+prebuf,:,1], + zCurr[1:i,:], NaN*ones(buffersize-i-prebuf,6))#[ones(buffersize-i-prebuf,1)*zCurr[i,1:5] zCurr[i,6]+collect(1:buffersize-i-prebuf)*dt*zCurr[i,1]]) + uCurr_export = cat(1,oldTraj.oldInput[oldTraj.oldCost[1]+1:oldTraj.oldCost[1]+prebuf,:,1], + uCurr[1:i,:], NaN*ones(buffersize-i-prebuf,2))#zeros(buffersize-i-prebuf,2)) + + zCurr_export[1:prebuf,6] -= posInfo.s_target # make the prebuf-values below zero + costLap = i # the cost of the current lap is the time it took to reach the finish line + println("Saving") + # Save all data in oldTrajectory: + if lapStatus.currentLap <= 2 # if it's the first or second lap + oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldTraj[:,:,2] = zCurr_export + oldTraj.oldInput[:,:,2] = uCurr_export + oldTraj.oldCost = [costLap,costLap] + else # idea: always copy the new trajectory in the first array! + if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second + oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second + oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input + oldTraj.oldCost[2] = oldTraj.oldCost[1] + end + oldTraj.oldTraj[:,:,1] = zCurr_export # ... and write the new traj in the first + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldCost[1] = costLap + end +end + +function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, + posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) + mpcParams.N = 12 + mpcParams.Q = [5.0,0.0,0.0,1.0,10.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 0.8 # reference speed for first lap of path following + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 1.0*[1,1,1,1,1,0] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[5.0,40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 2.0 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + + mpcParams_pF.N = 15 + mpcParams_pF.Q = [0.0,50.0,0.1,10.0] + mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f + mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states + mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs + mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following + mpcParams_pF.delay_df = 3 # steering delay (number of steps) + mpcParams_pF.delay_a = 1 # acceleration delay + + trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation + trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) + trackCoeff.width = 0.6 # width of the track (0.5m) + + modelParams.l_A = 0.125 + modelParams.l_B = 0.125 + modelParams.dt = 0.1 # sampling time, also controls the control loop, affects delay_df and Qderiv + modelParams.m = 1.98 + modelParams.I_z = 0.03 + modelParams.c_f = 0.5 # friction coefficient: xDot = - c_f*xDot (aerodynamic+tire) + + posInfo.s_target = 5.0 + + oldTraj.oldTraj = NaN*ones(buffersize,7,30) + oldTraj.oldInput = zeros(buffersize,2,30) + oldTraj.oldTimes = NaN*ones(buffersize,30) + oldTraj.count = ones(30)*2 + oldTraj.oldCost = ones(Int64,30) # dummies for initialization + oldTraj.prebuf = 30 + oldTraj.postbuf = 30 + oldTraj.idx_start = zeros(30) + oldTraj.idx_end = zeros(30) + + mpcCoeff.order = 5 + mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) + mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,5) + mpcCoeff.pLength = 5*2*mpcParams.N # small values here may lead to numerical problems since the functions are only approximated in a short horizon + mpcCoeff.c_Vx = zeros(3) + mpcCoeff.c_Vy = zeros(4) + mpcCoeff.c_Psi = zeros(3) + + lapStatus.currentLap = 1 # initialize lap number + lapStatus.currentIt = 0 # current iteration in lap +end + +# Use this function to smooth data (moving average) +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl new file mode 100644 index 00000000..eabdab28 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -0,0 +1,112 @@ +# Variable definitions +# mdl.z_Ol[i,j] = z_OpenLoop, open loop prediction of the state, i = state, j = step + +# States in path following mode: +# i = 1 -> s +# i = 2 -> ey +# i = 3 -> epsi +# i = 4 -> v + +# States in LMPC and system ID mode: +# i = 1 -> xDot +# i = 2 -> yDot +# i = 3 -> psiDot +# i = 4 -> ePsi +# i = 5 -> eY +# i = 6 -> s + +function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64}) + + # Load Parameters + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + # Update current initial condition, curvature and System ID coefficients + setvalue(mdl.z0,zCurr) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.c_Vx,mpcCoeff.c_Vx) # System ID coefficients + setvalue(mdl.c_Vy,mpcCoeff.c_Vy) + setvalue(mdl.c_Psi,mpcCoeff.c_Psi) + setvalue(mdl.coeff,trackCoeff.coeffCurvature) # Track curvature + setvalue(mdl.coeffTermCost,mpcCoeff.coeffCost) # Terminal cost + setvalue(mdl.coeffTermConst,mpcCoeff.coeffConst) # Terminal constraints + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + + # export data + mpcSol.a_x = sol_u[1,1] + mpcSol.d_f = sol_u[1,2] + mpcSol.u = sol_u + mpcSol.z = sol_z + mpcSol.solverStatus = sol_status + mpcSol.cost = zeros(6) + mpcSol.cost = [0,getvalue(mdl.costZTerm),getvalue(mdl.constZTerm),getvalue(mdl.derivCost),0,getvalue(mdl.laneCost)] + #mpcSol.cost = [getvalue(mdl.costZ),0,0,getvalue(mdl.derivCost),0,0] + + # Print information + # println("--------------- MPC START -----------------------------------------------") + # println("z0 = $(zCurr')") + # #println("z_Ol = $(sol_z)") + # #println("u_Ol = $(sol_u)") + # println("c_Vx = $(mpcCoeff.c_Vx)") + # println("c_Vy = $(mpcCoeff.c_Vy)") + # println("c_Psi = $(getvalue(mdl.c_Psi))") + # println("ParInt = $(getvalue(mdl.ParInt))") + # #println("u_prev = $(getvalue(mdl.uPrev))") + println("Solved, status = $sol_status") + # println("Predict. to s = $(sol_z[end,6])") + # println("costZ = $(mpcSol.cost[1])") + # println("termCost = $(mpcSol.cost[2])") + # println("termConst = $(mpcSol.cost[3])") + # println("derivCost = $(mpcSol.cost[4])") + # println("controlCost = $(mpcSol.cost[5])") + # println("laneCost = $(mpcSol.cost[6])") + # println("costZ: epsi = $(norm(sol_z[:,4]))") + # println("costZ: ey = $(norm(sol_z[:,5]))") + # println("costZ: v = $(norm(sol_z[:,1]-0.8))") + # println("--------------- MPC END ------------------------------------------------") + nothing +end + +function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::MpcParams,trackCoeff::TrackCoeff,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64}) + + # Load Parameters + coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} + + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + # Update current initial condition, curvature and previous input + setvalue(mdl.z0,zCurr) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.coeff,coeffCurvature) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + + mpcSol.a_x = sol_u[1,1] + mpcSol.d_f = sol_u[1,2] + mpcSol.u = sol_u + mpcSol.z = sol_z + mpcSol.solverStatus = sol_status + mpcSol.cost = zeros(6) + #mpcSol.cost = [getvalue(mdl.costZ),0,0,getvalue(mdl.derivCost),getvalue(mdl.controlCost),0] + + # Print information + # println("--------------- MPC PF START -----------------------------------------------") + # println("z0 = $(zCurr')") + println("Solved, status = $sol_status") + # println("Predict. to s = $(sol_z[end,1])") + # #println("uPrev = $(uPrev)") + # println("--------------- MPC PF END ------------------------------------------------") + nothing +end + +# Cost structure: costZ, costZTerm, constZTerm, derivCost, controlCost, laneCost diff --git a/workspace/src/barc/src/barc_lib/README.md b/workspace/src/barc/src/barc_lib/README.md new file mode 100644 index 00000000..34a413cd --- /dev/null +++ b/workspace/src/barc/src/barc_lib/README.md @@ -0,0 +1,5 @@ +# barc_lib + +This repo contains functions and types that are used by both the Barc simulation framework and the "real" ROS framework. +It contains basic BARC-specific types (related to the model and to MPC functions) and functions for their initialization. There are also functions to simulate the car's dynamics and to control the car along a racetrack (using Learning Model Predictive Control). +The library should be structured in folders according to its purposes. \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl new file mode 100644 index 00000000..dfad9d97 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -0,0 +1,95 @@ +# VARIOUS TYPES FOR CALCULATIONS + +type LapStatus + currentLap::Int64 # current lap number + currentIt::Int64 # current iteration in current lap + switchLap::Bool + nextLap::Bool + s_lapTrigger::Float64 +end + +# Structure of coeffConst: +# 1st dimension is the polynomial coefficient +# 2nd dimension specifies the state (1 = eY, 2 = ePsi, 3 = v or 1 = xDot, 2 = yDot, 3 = psiDot, 4 = ePsi, 5 = eY) +# 3th dimension specifies one of the two lap numbers between which are iterated + +type MpcCoeff # coefficients for trajectory approximation + coeffCost::Array{Float64} + coeffConst::Array{Float64} + order::Int64 + pLength::Int64 # small values here may lead to numerical problems since the functions are only approximated in a short horizon + # "small" values are about 2*N, good values about 4*N + # numerical problems occur at the edges (s=0, when v is almost 0 and s does not change fast and at s=s_target) + c_Vx::Array{Float64,1} + c_Vy::Array{Float64,1} + c_Psi::Array{Float64,1} + MpcCoeff(coeffCost=Float64[], coeffConst=Float64[], order=4, pLength=0,c_Vx=Float64[],c_Vy=Float64[],c_Psi=Float64[]) = new(coeffCost, coeffConst, order, pLength, c_Vx, c_Vy, c_Psi) +end + +type OldTrajectory # information about previous trajectories + oldTraj::Array{Float64} # contains all states over all laps + oldInput::Array{Float64} # contains all inputs + oldTimes::Array{Float64} # contains times related to states and inputs + oldCost::Array{Int64} # contains costs of laps + count::Array{Int64} # contains the counter for each lap + prebuf::Int64 + postbuf::Int64 + idx_start::Array{Int64} # index of the first measurement with s > 0 + idx_end::Array{Int64} # index of the last measurement with s < s_target + OldTrajectory(oldTraj=Float64[],oldInput=Float64[],oldTimes=Float64[],oldCost=Float64[],count=Int64[],prebuf=50,postbuf=50,idx_start=Int64[],idx_end=Int64[]) = new(oldTraj,oldInput,oldTimes,oldCost,count,prebuf,postbuf,idx_start,idx_end) +end + +type MpcParams # parameters for MPC solver + N::Int64 + nz::Int64 + OrderCostCons::Int64 + Q::Array{Float64,1} + Q_term::Array{Float64,1} + R::Array{Float64,1} + vPathFollowing::Float64 + QderivZ::Array{Float64,1} + QderivU::Array{Float64,1} + Q_term_cost::Float64 + delay_df::Int64 + delay_a::Int64 + MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a) +end + +type PosInfo # current position information + s::Float64 + s_target::Float64 + PosInfo(s=0,s_target=0) = new(s,s_target) +end + +type MpcSol # MPC solution output + a_x::Float64 + d_f::Float64 + solverStatus::Symbol + u::Array{Float64} + z::Array{Float64} + cost::Array{Float64} + MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) +end + +type TrackCoeff # coefficients of track + coeffAngle::Array{Float64,1} + coeffCurvature::Array{Float64,1} + nPolyCurvature::Int64 # order of the interpolation polynom + width::Float64 # lane width -> is used in cost function as soft constraints (to stay on track) + TrackCoeff(coeffAngle=Float64[],coeffCurvature=Float64[],nPolyCurvature=4,width=1.0) = new(coeffAngle,coeffCurvature,nPolyCurvature,width) +end + +type ModelParams + l_A::Float64 + l_B::Float64 + m::Float64 + I_z::Float64 + dt::Float64 + u_lb::Array{Float64} # lower bounds for u + u_ub::Array{Float64} # upper bounds + z_lb::Array{Float64} + z_ub::Array{Float64} + c0::Array{Float64} + c_f::Float64 + ModelParams(l_A=0.25,l_B=0.25,m=1.98,I_z=0.24,dt=0.1,u_lb=Float64[],u_ub=Float64[],z_lb=Float64[],z_ub=Float64[],c0=Float64[],c_f=0.0) = new(l_A,l_B,m,I_z,dt,u_lb,u_ub,z_lb,z_ub,c0,c_f) +end diff --git a/workspace/src/barc/src/barc_lib/log_functions.jl b/workspace/src/barc/src/barc_lib/log_functions.jl new file mode 100644 index 00000000..78195fb8 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/log_functions.jl @@ -0,0 +1,75 @@ + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.t_msg = m.t_msg[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + +function Quat2Euler(q::Array{Float64}) + sol = zeros(Float64,3) + sol[1] = atan2(2*(q[1]*q[2]+q[3]*q[4]),1-2*(q[2]^2+q[3]^2)) + sol[2] = asin(2*(q[1]*q[3]-q[4]*q[2])) + sol[3] = atan2(2*(q[1]*q[4]+q[2]*q[3]),1-2*(q[3]^2+q[4]^2)) + return sol +end + +function ECU_callback(msg::ECU,cmd_log::Measurements) + cmd_log.t[cmd_log.i] = to_sec(get_rostime()) + cmd_log.t_msg[cmd_log.i] = to_sec(msg.header.stamp) + cmd_log.z[cmd_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) + cmd_log.i += 1 + nothing +end + +function ECU_PWM_callback(msg::ECU,cmd_pwm_log::Measurements) + cmd_pwm_log.t[cmd_pwm_log.i] = to_sec(get_rostime()) + cmd_pwm_log.t_msg[cmd_pwm_log.i] = to_sec(msg.header.stamp) + cmd_pwm_log.z[cmd_pwm_log.i,:] = convert(Array{Float64,1},[msg.motor;msg.servo]) + cmd_pwm_log.i += 1 + nothing +end + +function IMU_callback(msg::Imu,imu_meas::Measurements) + imu_meas.t[imu_meas.i] = to_sec(get_rostime()) + imu_meas.t_msg[imu_meas.i] = to_sec(msg.header.stamp) + imu_meas.z[imu_meas.i,:] = [msg.angular_velocity.x;msg.angular_velocity.y;msg.angular_velocity.z; + Quat2Euler([msg.orientation.w;msg.orientation.x;msg.orientation.y;msg.orientation.z]); + msg.linear_acceleration.x;msg.linear_acceleration.y;msg.linear_acceleration.z]::Array{Float64} + imu_meas.i += 1 + nothing +end + +function GPS_callback(msg::hedge_pos,gps_meas::Measurements) + gps_meas.t[gps_meas.i] = to_sec(get_rostime()) + gps_meas.t_msg[gps_meas.i] = to_sec(msg.header.stamp) + gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] + gps_meas.i += 1 + nothing +end + +function pos_info_callback(msg::pos_info,pos_info_log::Measurements) + pos_info_log.t[pos_info_log.i] = to_sec(get_rostime()) + pos_info_log.t_msg[pos_info_log.i] = to_sec(msg.header.stamp) + pos_info_log.z[pos_info_log.i,:] = [msg.s;msg.ey;msg.epsi;msg.v;msg.s_start;msg.x;msg.y;msg.v_x;msg.v_y; + msg.psi;msg.psiDot;msg.x_raw;msg.y_raw;msg.psi_raw;msg.v_raw;msg.psi_drift; + msg.a_x;msg.a_y;msg.a_x_raw;msg.a_y_raw] + pos_info_log.i += 1 + nothing +end + +function vel_est_callback(msg::Vel_est,vel_est_log::Measurements) + vel_est_log.t[vel_est_log.i] = to_sec(get_rostime()) + vel_est_log.t_msg[vel_est_log.i] = to_sec(msg.header.stamp) + vel_est_log.z[vel_est_log.i,:] = [msg.vel_est,msg.vel_fl,msg.vel_fr,msg.vel_bl,msg.vel_br] + vel_est_log.i += 1 + nothing +end diff --git a/workspace/src/barc/src/barc_lib/simModel.jl b/workspace/src/barc/src/barc_lib/simModel.jl new file mode 100644 index 00000000..bdf7fa5c --- /dev/null +++ b/workspace/src/barc/src/barc_lib/simModel.jl @@ -0,0 +1,138 @@ +# z[1] = xDot +# z[2] = yDot +# z[3] = psiDot +# z[4] = ePsi +# z[5] = eY +# z[6] = s + +function simKinModel(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + + zNext::Array{Float64} + L_a = modelParams.l_A + L_b = modelParams.l_B + c = ([z[1]^8 z[1]^7 z[1]^6 z[1]^5 z[1]^4 z[1]^3 z[1]^2 z[1] 1]*coeff)[1] # Polynomial + + bta = atan(L_a/(L_a+L_b)*tan(u[2]+abs(u[2])*u[2])) + dsdt = z[4]*cos(z[3]+bta)/(1-z[2]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt*dsdt # s + zNext[2] = z[2] + dt*z[4] * sin(z[3] + bta) # eY + zNext[3] = z[3] + dt*(z[4]/L_a*sin(bta)-dsdt*c) # ePsi + zNext[4] = z[4] + dt*(u[1] - modelParams.c_f*z[4]) # v + + return zNext +end + +function simDynModel_exact(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + # This function uses smaller steps to achieve higher fidelity than we would achieve using longer timesteps + z_final = copy(z) + u[1] = min(u[1],3) + u[1] = max(u[1],-3) + u[2] = min(u[2],pi/6) + u[2] = max(u[2],-pi/6) + dtn = dt/100 + for i=1:100 + z_final = simDynModel(z_final,u,dtn,coeff,modelParams) + end + return z_final +end + +function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + c0 = modelParams.c0 + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f + + a_F = 0 + a_R = 0 + if abs(z[1]) >= 0.1 + a_F = atan((z[2] + L_f*z[3])/abs(z[1])) - z[8] + a_R = atan((z[2] - L_r*z[3])/abs(z[1])) + end + if max(abs(a_F),abs(a_R))>30/180*pi + warn("Large tire angles: a_F = $a_F, a_R = $a_R, xDot = $(z[1]), d_F = $(z[8])") + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + c = ([z[1]^8 z[2]^7 z[3]^6 z[4]^5 z[5]^4 z[6]^3 z[7]^2 z[8] 1]*coeff)[1] # Polynomial for curvature + + dsdt = (z[1]*cos(z[4]) - z[2]*sin(z[4]))/(1-z[5]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt * (z[7] + z[2]*z[3] - c_f*z[1]) # xDot + zNext[2] = z[2] + dt * (2/m*(FyF*cos(z[8]) + FyR) - z[3]*z[1]) # yDot + zNext[3] = z[3] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) # psiDot + zNext[4] = z[4] + dt * (z[3]-dsdt*c) # ePsi + zNext[5] = z[5] + dt * (z[1]*sin(z[4]) + z[2]*cos(z[4])) # eY + zNext[6] = z[6] + dt * dsdt # s + zNext[7] = z[7] + dt * (u[1] - z[7]) * 100 # a + zNext[8] = z[8] + dt * (u[2] - z[8]) * 10 # d_f + + return zNext +end + +function pacejka(a) + B = 1.0 # This value determines the steepness of the curve + C = 1.25 + mu = 0.8 # Friction coefficient (responsible for maximum lateral tire force) + m = 1.98 + g = 9.81 + D = mu * m * g/2 + C_alpha_f = D*sin(C*atan(B*a)) + return C_alpha_f +end + +function simDynModel_exact_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + dtn = dt/10 + t = 0:dtn:dt + z_final = copy(z) + ang = zeros(2) + for i=1:length(t)-1 + z_final, ang = simDynModel_xy(z_final,u,dtn,modelParams) + end + return z_final, ang +end + +function simDynModel_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + + a_F = 0.0 + a_R = 0.0 + if abs(z[3]) > 0.2 + a_F = atan((z[4] + L_f*z[6])/abs(z[3])) - z[8] + a_R = atan((z[4] - L_r*z[6])/abs(z[3])) + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + if abs(a_F) > 30/180*pi || abs(a_R) > 30/180*pi + warn("Large slip angles in simulation: a_F = $a_F, a_R = $a_R") + end + + zNext = copy(z) + # compute next state + zNext[1] = zNext[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) # x + zNext[2] = zNext[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) # y + zNext[3] = zNext[3] + dt * (z[7] + z[4]*z[6] - 0.5*z[3]) # v_x + zNext[4] = zNext[4] + dt * (2/m*(FyF*cos(z[8]) + FyR) - z[6]*z[3]) # v_y + zNext[5] = zNext[5] + dt * (z[6]) # psi + zNext[6] = zNext[6] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) # psiDot + zNext[7] = zNext[7] + dt * (u[1]-z[7])*100 # a + zNext[8] = zNext[8] + dt * (u[2]-z[8])*100 # d_f + + zNext[3] = max(0,zNext[3]) # limit speed to positive values (BARC specific) + return zNext, [a_F a_R] +end From ad1878ba7bb4f656fa6b0e1a7a1a1cc78328a7ae Mon Sep 17 00:00:00 2001 From: maxb91 Date: Thu, 8 Dec 2016 16:22:35 -0800 Subject: [PATCH 107/183] Cleaned source code, added Readme file. --- .../barc/launch/estimation_DynBkMdl.launch | 42 --- .../barc/launch/estimation_KinBkMdl.launch | 35 -- .../src/barc/launch/openLoop_circular.launch | 48 --- .../src/barc/launch/openLoop_straight.launch | 43 --- .../src/barc/launch/run_lmpc_barc.launch | 47 --- workspace/src/barc/src/README.md | 56 ++++ workspace/src/barc/src/barc_simulator.jl | 205 ------------ workspace/src/barc/src/controller_circular.py | 78 ----- workspace/src/barc/src/controller_straight.py | 146 --------- workspace/src/barc/src/pid.py | 96 ------ .../src/barc/src/state_estimation_DynBkMdl.py | 257 --------------- .../src/state_estimation_DynBkMdl_mixed.py | 201 ------------ .../src/barc/src/state_estimation_KinBkMdl.py | 244 -------------- .../src/state_estimation_KinBkMdl_mixed.py | 248 --------------- ...ate_estimation_SensorKinematicModel_sim.py | 299 ------------------ .../barc/src/state_estimation_SensorModel.py | 254 --------------- workspace/src/barc/src/system_models.py | 179 ++--------- workspace/src/barc/src/testCoeffConstr.jl | 55 ---- 18 files changed, 80 insertions(+), 2453 deletions(-) delete mode 100644 workspace/src/barc/launch/estimation_DynBkMdl.launch delete mode 100755 workspace/src/barc/launch/estimation_KinBkMdl.launch delete mode 100644 workspace/src/barc/launch/openLoop_circular.launch delete mode 100644 workspace/src/barc/launch/openLoop_straight.launch delete mode 100644 workspace/src/barc/launch/run_lmpc_barc.launch create mode 100644 workspace/src/barc/src/README.md delete mode 100755 workspace/src/barc/src/barc_simulator.jl delete mode 100755 workspace/src/barc/src/controller_circular.py delete mode 100755 workspace/src/barc/src/controller_straight.py delete mode 100644 workspace/src/barc/src/pid.py delete mode 100755 workspace/src/barc/src/state_estimation_DynBkMdl.py delete mode 100755 workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py delete mode 100755 workspace/src/barc/src/state_estimation_KinBkMdl.py delete mode 100755 workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py delete mode 100755 workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py delete mode 100755 workspace/src/barc/src/state_estimation_SensorModel.py delete mode 100644 workspace/src/barc/src/testCoeffConstr.jl diff --git a/workspace/src/barc/launch/estimation_DynBkMdl.launch b/workspace/src/barc/launch/estimation_DynBkMdl.launch deleted file mode 100644 index cab1838a..00000000 --- a/workspace/src/barc/launch/estimation_DynBkMdl.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/estimation_KinBkMdl.launch b/workspace/src/barc/launch/estimation_KinBkMdl.launch deleted file mode 100755 index 7640020d..00000000 --- a/workspace/src/barc/launch/estimation_KinBkMdl.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/openLoop_circular.launch b/workspace/src/barc/launch/openLoop_circular.launch deleted file mode 100644 index 44d3d417..00000000 --- a/workspace/src/barc/launch/openLoop_circular.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/openLoop_straight.launch b/workspace/src/barc/launch/openLoop_straight.launch deleted file mode 100644 index 56142084..00000000 --- a/workspace/src/barc/launch/openLoop_straight.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/run_lmpc_barc.launch b/workspace/src/barc/launch/run_lmpc_barc.launch deleted file mode 100644 index fce0bfa8..00000000 --- a/workspace/src/barc/launch/run_lmpc_barc.launch +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/src/README.md b/workspace/src/barc/src/README.md new file mode 100644 index 00000000..9334e211 --- /dev/null +++ b/workspace/src/barc/src/README.md @@ -0,0 +1,56 @@ +Source code for LMPC +==================== + +1. barc_record.jl +================= +This node records data that might be interesting for later evaluation. It subscribes to topics, records received messages into arrays and saves these arrays into a .jld file when the ros master is shut down. It uses log_functions.jl in barc_lib to import the callback functions. + +2. barc_simulator_dyn.jl +======================== +This node simulates a dynamic bicycle model. It subscribes to the ECU commands of the MPC and publishes simulated "raw" measurement data: GPS, IMU and Encoders. It also logs its "real" states into a .jld file, this can be interesting to experiment with Kalman filters to find a good estimator (since you can compare "real" simulation data with your estimation data). +It uses the simModel.jl in barc_lib. + +3. controller_low_level.py +========================== +This is the low level controller that maps acceleration and steering angle commands (in SI units) to PWM signals that are needed by the motor and steering servo. The mapping is linear and has been found by multiple open-loop measurements. It might vary from BARC to BARC. + +4. controller_MPC_traj.jl +========================= +This is a simple MPC path following controller that uses a given track. + +5. debug_localization.py +======================== +This is a very short script you can use to debug and analyze the mapping from x/y coordinates to s/e_y/e_psi. You can use it to find an optimal approximation length for the curvature. + +6. filtering.py +=============== +Basic filtering classes, not used in LMPC but might be helpful for future use. + +7. LMPC_node.jl +=============== +This is the main node of LMPC. It receives data from the estimator, records it and does system ID and LMPC to find optimal inputs. + +8. Localization_helpers.py +========================== +This contains a class that is used to map x-y-coordinates to s-ey-epsi coordinates. It also includes the shape of the track (in create_track()). Tracks are created by putting together curves using the function add_curve(track,length,angle_of_curve). This makes sure that there are no jumps in curvature (but there are jumps in the 1st derivative of the curvature!). +Mapping x-y to s-ey: +1. set_pos() sets the class variables to the current position/heading +2. find_s() calculates s, ey, epsi and the curvature coefficients according to its settings (polynomial degree, length of approximation) +3. read variables (s,ey, ...) manually from class (look at state estimation node to get an idea) + +9. observers.py +=============== +Contains the function for the EKF and other necessary functions. + +10. open_loop.jl +================ +Can do open loop experiments with this and send either ECU or ECU_pwm commands. + +11. state_estimation_SensorKinematicModel.py +============================================ +State estimation node. Uses a combination of a kinematic bicycle model and a kinematic motion model in an EKF (without the bicycle assumption) to calculate states. It also prefilters GPS data to make sure holes and outliers are not used. + +12. system_models.py +==================== +This contains the model that is used by the Kalman filter in state estimation. + diff --git a/workspace/src/barc/src/barc_simulator.jl b/workspace/src/barc/src/barc_simulator.jl deleted file mode 100755 index 88ab6152..00000000 --- a/workspace/src/barc/src/barc_simulator.jl +++ /dev/null @@ -1,205 +0,0 @@ -#!/usr/bin/env julia - -#= - Licensing Information: You are free to use or extend these projects for - education or reserach purposes provided that (1) you retain this notice - and (2) you provide clear attribution to UC Berkeley, including a link - to http://barc-project.com - - Attibution Information: The barc project ROS code-base was developed - at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales - (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed - by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was - based on an open source project by Bruce Wootton -=# - -using RobotOS -@rosimport barc.msg: ECU, Encoder, Ultrasound, Z_KinBkMdl, Logging, Z_DynBkMdl -@rosimport data_service.msg: TimeData -@rosimport geometry_msgs.msg: Vector3 -@rosimport sensor_msgs.msg: Imu -rostypegen() -using barc.msg -using data_service.msg -using geometry_msgs.msg -using sensor_msgs.msg -using JLD - -include("barc_lib/simModel.jl") - -u_current = zeros(Float64,2,1) # msg ECU is Float32 ! - -t = 0 - - -# This type contains measurement data (time, values and a counter) -type Measurements{T} - i::Int64 # measurement counter - t::Array{Float64} # time data - z::Array{T} # measurement values -end -# This function cleans the zeros from the type above once the simulation is finished -function clean_up(m::Measurements) - m.t = m.t[1:m.i-1] - m.z = m.z[1:m.i-1,:] -end - -buffersize = 60000 -gps_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -imu_meas = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -est_meas = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,4)) -est_meas_dyn = Measurements{Float32}(0,zeros(buffersize),zeros(Float32,buffersize,6)) -cmd_log = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,2)) -z_real = Measurements{Float64}(0,zeros(buffersize),zeros(buffersize,4)) - -z_real.t[1] = time() -imu_meas.t[1] = time() -est_meas.t[1] = time() -est_meas_dyn.t[1] = time() -cmd_log.t[1] = time() - -function simModel(z,u,dt,l_A,l_B) - - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - - zNext = z - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v - - # Add process noise (depending on velocity) - zNext = zNext + diagm([0.001*z[4],0.001*z[4],0.00,0.001*z[4]])*randn(4,1) - - return zNext -end - - -function ECU_callback(msg::ECU) - global u_current - u_current = [msg.motor, msg.servo] - cmd_log.i += 1 - cmd_log.t[cmd_log.i] = time() - cmd_log.z[cmd_log.i,:] = u_current' -end - -function est_dyn_callback(msg::Z_DynBkMdl) - global est_meas_dyn, est_meas - est_meas_dyn.i += 1 - est_meas_dyn.t[est_meas_dyn.i] = time() - est_meas_dyn.z[est_meas_dyn.i,:] = [msg.x msg.y msg.v_x msg.v_y msg.psi msg.psi_dot] - est_meas.i += 1 - est_meas.t[est_meas.i] = time() - est_meas.z[est_meas.i,:] = [msg.x msg.y msg.psi msg.v_x] -end - -function main() - # initiate node, set up publisher / subscriber topics - init_node("barc_sim") - pub_enc = Publisher("encoder", Encoder, queue_size=10) - pub_gps = Publisher("indoor_gps", Vector3, queue_size=10) - pub_imu = Publisher("imu/data", Imu, queue_size=10) - - - # read the axle distances from the launch file - l_A = get_param("L_a") # distance from CoG to front axel - l_B = get_param("L_b") # distance from CoG to rear axel - - s1 = Subscriber("ecu", ECU, ECU_callback, queue_size=10) - s3 = Subscriber("state_estimate_dynamic", Z_DynBkMdl, est_dyn_callback, queue_size=10) - - z_current = zeros(60000,4) - z_current[1,:] = [0.2 0.0 0.0 0.0] - - dt = 0.01 - loop_rate = Rate(1/dt) - - i = 2 - - dist_traveled = 0 - last_updated = 0 - - r_tire = 0.036 # radius from tire center to perimeter along magnets [m] - quarterCirc = 0.5 * pi * r_tire # length of a quarter of a tire, distance from one to the next encoder - - FL = 0 #front left wheel encoder counter - FR = 0 #front right wheel encoder counter - BL = 0 #back left wheel encoder counter - BR = 0 #back right wheel encoder counter - - imu_drift = 0 # simulates yaw-sensor drift over time (slow sine) - - println("Publishing sensor information. Simulator running.") - while ! is_shutdown() - - t = time() - # update current state with a new row vector - z_current[i,:] = simModel(z_current[i-1,:]',u_current, dt, l_A,l_B)' - z_real.t[i] = t - - # Encoder measurements calculation - dist_traveled += z_current[i,4]*dt #count the total traveled distance since the beginning of the simulation - if dist_traveled - last_updated >= quarterCirc - last_updated = dist_traveled - FL += 1 - FR += 1 - BL += 0 #no encoder on back left wheel - BR += 0 #no encoder on back right wheel - enc_data = Encoder(FL, FR, BL, BR) - publish(pub_enc, enc_data) #publish a message everytime the encoder counts up - end - - # IMU measurements - imu_data = Imu() - imu_drift = sin(t/100*pi/2) # drifts to 1 in 100 seconds - yaw = z_current[i,3] + 0*(randn()*0.05 + imu_drift) - yaw_dot = (z_current[i,3]-z_current[i-1,3])/dt + 0.05*randn() - imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) - imu_data.angular_velocity = Vector3(0,0,yaw_dot) - if i%2 == 0 - imu_meas.i += 1 - imu_meas.t[imu_meas.i] = t - imu_meas.z[imu_meas.i,:] = [yaw yaw_dot] - publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" - # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) - end - - # GPS measurements - x = round(z_current[i,1]*100 + 1*randn()*2) # Indoor gps measures in cm - y = round(z_current[i,2]*100 + 1*randn()*2) - if i % 7 == 0 - gps_meas.i += 1 - gps_meas.t[gps_meas.i] = t - gps_meas.z[gps_meas.i,:] = [x y] - gps_data = Vector3(x,y,0) - publish(pub_gps, gps_data) - end - - i += 1 - rossleep(loop_rate) - end - - # Clean up buffers - - clean_up(gps_meas) - clean_up(est_meas) - clean_up(est_meas_dyn) - clean_up(imu_meas) - clean_up(cmd_log) - z_real.z[1:i-1,:] = z_current[1:i-1,:] - z_real.i = i - clean_up(z_real) - # Save simulation data to file - log_path = "$(homedir())/simulations/output.jld" - save(log_path,"gps_meas",gps_meas,"z",z_real,"estimate",est_meas,"estimate_dyn",est_meas_dyn,"imu_meas",imu_meas,"cmd_log",cmd_log) - println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") - #writedlm(log_path,z_current[1:i-1,:]) -end - -if ! isinteractive() - main() -end diff --git a/workspace/src/barc/src/controller_circular.py b/workspace/src/barc/src/controller_circular.py deleted file mode 100755 index 3c06ed07..00000000 --- a/workspace/src/barc/src/controller_circular.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from rospy import init_node, Subscriber, Publisher, get_param -from rospy import Rate, is_shutdown, ROSInterruptException -from barc.msg import ECU -from numpy import pi -import rospy - -############################################################# -def circular(t_i, t_0, t_f, d_f_target, FxR_target): - # rest - if t_i < t_0: - d_f = 0 - FxR = 0 - - # start moving - elif (t_i < t_f): - d_f = d_f_target - FxR = FxR_target - - # stop experiment - else: - d_f = 0 - FxR = 0 - - return (FxR, d_f) - - -############################################################# -def main_auto(): - # initialize ROS node - init_node('auto_mode', anonymous=True) - nh = Publisher('ecu', ECU, queue_size = 10) - - # set node rate - rateHz = get_param("controller/rate") - rate = Rate(rateHz) - dt = 1.0 / rateHz - t_i = 0 - - # get experiment parameters - t_0 = get_param("controller/t_0") # time to start test - t_f = get_param("controller/t_f") # time to end test - FxR_target = get_param("controller/FxR_target") - d_f_target = pi/180*get_param("controller/d_f_target") - - # main loop - while not is_shutdown(): - # get command signal - (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target) - - # send command signal - ecu_cmd = ECU(FxR, d_f) - nh.publish(ecu_cmd) - - # wait - t_i += dt - rate.sleep() - -############################################################# -if __name__ == '__main__': - try: - main_auto() - except ROSInterruptException: - pass diff --git a/workspace/src/barc/src/controller_straight.py b/workspace/src/barc/src/controller_straight.py deleted file mode 100755 index 3a910206..00000000 --- a/workspace/src/barc/src/controller_straight.py +++ /dev/null @@ -1,146 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from rospy import init_node, Subscriber, Publisher, get_param -from rospy import Rate, is_shutdown, ROSInterruptException -from barc.msg import ECU, Z_KinBkMdl -from sensor_msgs.msg import Imu -from math import pi -from numpy import zeros, array -from numpy import unwrap -from tf import transformations -from pid import PID -import numpy as np -import rospy - -# raw measurement variables -yaw_prev = 0 -(roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) -yaw_prev = 0 -yaw_local = 0 -read_yaw0 = False - -# imu measurement update -def imu_callback(data): - # units: [rad] and [rad/s] - global yaw - global yaw_prev, yaw0, read_yaw0, yaw_local - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (_,_, yaw) = transformations.euler_from_quaternion(quaternion) - - # save initial measurements - if not read_yaw0: - read_yaw0 = True - yaw_prev = yaw - yaw0 = yaw - - # unwrap measurement - yaw = unwrap(array([yaw_prev, yaw]), discont = pi)[1] - yaw_prev = yaw - yaw_local = yaw - yaw0 - - # extract angular velocity and linear acceleration data - # w_z = data.angular_velocity.z - - -############################################################ -def se_callback(data): - global yaw, read_yaw0 - yaw = data.psi - - if not read_yaw0: - read_yaw0 = True - -############################################################# -def straight(t_i, pid, time_params,FxR_target): - global yaw_local - - # unpack parameters - (t_0, t_f, dt) = time_params - - # rest - if t_i < t_0: - d_f = 0 - FxR = 0 - - # start moving - elif (t_i < t_f): - d_f = pid.update(yaw_local, dt) - step_up = float(t_i - t_0) / 50.0 - FxR = np.min([ step_up, FxR_target]) - - # stop experiment - else: - d_f = 0 - FxR = 0 - - return (FxR, d_f) - -############################################################# -def main_auto(): - global read_yaw0, yaw_local - - # initialize ROS node - init_node('auto_mode', anonymous=True) - ecu_pub = Publisher('ecu', ECU, queue_size = 10) - se_sub = Subscriber('imu/data', Imu, imu_callback) - - # set node rate - rateHz = get_param("controller/rate") - rate = Rate(rateHz) - dt = 1.0 / rateHz - - # get PID parameters - p = get_param("controller/p") - i = get_param("controller/i") - d = get_param("controller/d") - pid = PID(P=p, I=i, D=d) - setReference = False - - # get experiment parameters - t_0 = get_param("controller/t_0") # time to start test - t_f = get_param("controller/t_f") # time to end test - FxR_target = get_param("controller/FxR_target") - t_params = (t_0, t_f, dt) - - while not is_shutdown(): - - # OPEN LOOP - if read_yaw0: - # set reference angle - if not setReference: - pid.setPoint(yaw_local) - setReference = True - t_i = 0.0 - # apply open loop command - else: - (FxR, d_f) = straight(t_i, pid, t_params, FxR_target) - ecu_cmd = ECU(FxR, d_f) - ecu_pub.publish(ecu_cmd) - t_i += dt - - # wait - rate.sleep() - -############################################################# -if __name__ == '__main__': - try: - main_auto() - except ROSInterruptException: - pass diff --git a/workspace/src/barc/src/pid.py b/workspace/src/barc/src/pid.py deleted file mode 100644 index 3b8d80ae..00000000 --- a/workspace/src/barc/src/pid.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -class PID: - def __init__(self, P=2.0, I=0.0, D=0, de=0, e_int = 0, Integrator_max=500, Integrator_min=-500): - self.Kp = P # proportional gain - self.Ki = I # integral gain - self.Kd = D # derivative gain - - self.set_point = 0 # reference point - self.e = 0 - - self.e_int = 0 - self.int_e_max = Integrator_max - self.int_e_min = Integrator_min - - self.current_value = 0 - - def update(self,current_value, dt): - self.current_value = current_value - - e_t = self.set_point - current_value - de_t = ( e_t - self.e)/dt - - self.e_int = self.e_int + e_t * dt - if self.e_int > self.int_e_max: - self.e_int = self.int_e_max - elif self.e_int < self.int_e_min: - self.e_int = self.int_e_min - - P_val = self.Kp * e_t - I_val = self.Ki * self.e_int - D_val = self.Kd * de_t - - PID = P_val + I_val + D_val - self.e = e_t - - return PID - - def setPoint(self,set_point): - self.set_point = set_point - # reset error integrator - self.e_int = 0 - # reset error, otherwise de/dt will skyrocket - self.e = set_point - self.current_value - - def setKp(self,P): - self.Kp=P - - def setKi(self,I): - self.Ki=I - - def setKd(self,D): - self.Kd=D - - def getPoint(self): - return self.set_point - - def getError(self): - return self.e - -#%% Example function -def fx(x, u, dt): - x_next = x + (3*x + u) * dt - return x_next - -#%% Test script to ensure program is functioning properly -if __name__ == "__main__": - - from numpy import zeros - import matplotlib.pyplot as plt - - n = 200 - x = zeros(n) - x[0] = 20 - pid = PID(P=3.7, I=5, D=0.5) - dt = 0.1 - - for i in range(n-1): - u = pid.update(x[i], dt) - x[i+1] = fx(x[i],u, dt) - - plt.plot(x) - plt.show() diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py deleted file mode 100755 index e06ed404..00000000 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ /dev/null @@ -1,257 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -from Localization_helpers import Localization -from barc.msg import ECU, pos_info, Vel_est -from sensor_msgs.msg import Imu -from marvelmind_nav.msg import hedge_pos -from std_msgs.msg import Header -from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, unwrap -from observers import ekf -from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_psi_drift, h_KinBkMdl_psi_drift -from tf import transformations - -# ***_meas are values that are used by the Kalman filters -# ***_raw are raw values coming from the sensors - -class StateEst(object): - """This class contains all variables that are read from the sensors and then passed to - the Kalman filter.""" - # input variables - cmd_servo = 0 - cmd_motor = 0 - - # IMU - yaw_prev = 0 - yaw0 = 0 # yaw at t = 0 - yaw_meas = 0 - imu_times = [0]*25 - psiDot_hist = [0]*25 - - # Velocity - vel_meas = 0 - - # GPS - x_meas = 0 - y_meas = 0 - - # General variables - t0 = 0 # Time when the estimator was started - running = False # bool if the car is driving - - def __init__(self): - self.x_meas = 0 - - # ecu command update - def ecu_callback(self, data): - self.cmd_motor = data.motor # input motor force [N] - self.cmd_servo = data.servo # input steering angle [rad] - if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero - self.running = True - - # ultrasound gps data - def gps_callback(self, data): - # units: [rad] and [rad/s] - #current_t = rospy.get_rostime().to_sec() - self.x_meas = data.x_m - self.y_meas = data.y_m - - # imu measurement update - def imu_callback(self, data): - # units: [rad] and [rad/s] - current_t = rospy.get_rostime().to_sec() - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) - # yaw_meas is element of [-pi,pi] - yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) - self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping - # from this point on 'yaw' will be definitely unwrapped (smooth)! - if not self.running: - self.yaw0 = yaw # set yaw0 to current yaw - self.yaw_meas = 0 # and current yaw to zero - else: - self.yaw_meas = yaw - self.yaw0 - - #imu_times.append(data.header.stamp.to_sec()-t0) - self.imu_times.append(current_t-self.t0) - self.imu_times.pop(0) - - # extract angular velocity and linear acceleration data - #w_x = data.angular_velocity.x - #w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - #a_x = data.linear_acceleration.x - #a_y = data.linear_acceleration.y - #a_z = data.linear_acceleration.z - - self.psiDot_hist.append(w_z) - self.psiDot_hist.pop(0) - - def vel_est_callback(self, data): - if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement - self.vel_meas = data.vel_est - -# state estimation node -def state_estimation(): - se = StateEst() - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, se.imu_callback) - rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) - rospy.Subscriber('ecu', ECU, se.ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) - state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) - - # get vehicle dimension parameters - L_f = rospy.get_param("L_a") # distance from CoG to front axel - L_r = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_f, L_r) - - # get EKF observer properties - psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation - gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode - - # set node rate - loop_rate = 25 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - se.t0 = rospy.get_rostime().to_sec() - - # settings about psi estimation (use different models accordingly) - psi_drift_active = True - psi_drift = 0 - - if psi_drift_active: - z_EKF = zeros(5) # x, y, psi, v, psi_drift - P = eye(5) # initial dynamics coveriance matrix - Q = diag([2.5, 2.5, 2.5, 2.5, 0.00025])*dt - else: - z_EKF = zeros(4) - P = eye(4) # initial dynamics coveriance matrix - Q = diag([2.5, 2.5, 2.5, 2.5])*dt - - if est_mode == 1: # use gps, IMU, and encoder - print "Using GPS, IMU and encoders." - R = diag([gps_std, gps_std, psi_std, v_std])**2 - elif est_mode == 2: # use IMU and encoder only - print "Using IMU and encoders." - R = diag([psi_std, v_std])**2 - elif est_mode == 3: # use gps only - print "Using GPS." - R = (gps_std**2)*eye(2) - elif est_mode == 4: # use gps and encoder - print "Using GPS and encoders." - R = diag([gps_std, gps_std, v_std])**2 - else: - rospy.logerr("No estimation mode selected.") - - # Set up track parameters - l = Localization() - l.create_track() - l.prepare_trajectory(0.06) - - d_f = 0 - - # Estimation variables - (x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est, v_est, psi_drift_est) = [0]*8 - psi_dot_meas = 0 - - while not rospy.is_shutdown(): - ros_t = rospy.get_rostime() - #t = ros_t.to_sec()-se.t0 # current time - - # calculate new steering angle (low pass filter on steering input to make v_y and v_x smoother) - d_f = d_f + (se.cmd_servo-d_f)*0.25 - - # GPS measurement update - sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - # make R values dependent on current measurement - R[0,0] = 1+10*sq_gps_dist - R[1,1] = 1+10*sq_gps_dist - - # update IMU polynomial: - #t_matrix_imu = vstack((imu_times,imu_times,ones(size(imu_times)))) - #t_matrix_imu[0] = t_matrix_imu[0]**2 - #poly_psiDot = linalg.lstsq(t_matrix_imu.T, psiDot_hist)[0] - #psiDot_meas_pred = polyval(poly_psiDot, t) - psi_dot_meas = se.psiDot_hist[-1] - - if psi_drift_active: - (x_est, y_est, psi_est, v_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate - else: - (x_est, y_est, psi_est, v_est) = z_EKF # note, r = EKF estimate yaw rate - - # use Kalman values to predict state in 0.1s - bta = arctan(L_f/(L_f+L_r)*tan(d_f)) - #x_pred = x# + dt_pred*(v*cos(psi + bta)) - #y_pred = y# + dt_pred*(v*sin(psi + bta)) - #psi_pred = psi# + dt_pred*v/L_r*sin(bta) - #v_pred = v# + dt_pred*(FxR - 0.63*sign(v)*v**2) - v_x_est = cos(bta)*v_est - v_y_est = sin(bta)*v_est - - # Update track position - l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) # v = v_x - #l.find_s() - l.s = 0 - l.epsi = 0 - l.s_start = 0 - - # and then publish position info - state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - l.coeffX.tolist(), l.coeffY.tolist(), - l.coeffTheta.tolist(), l.coeffCurvature.tolist())) - # get measurement - if est_mode == 1: - y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas]) - elif est_mode == 2: - y = array([se.yaw_meas, se.vel_meas]) - elif est_mode == 3: - y = array([se.x_meas, se.y_meas]) - elif est_mode == 4: - y = array([se.x_meas, se.y_meas, se.vel_meas]) - else: - print "Wrong estimation mode specified." - - # define input - u = array([d_f, se.cmd_motor]) - - # build extra arguments for non-linear function - args = (u, vhMdl, dt, est_mode) - - # apply EKF and get each state estimate - if psi_drift_active: - (z_EKF, P) = ekf(f_KinBkMdl_psi_drift, z_EKF, P, h_KinBkMdl_psi_drift, y, Q, R, args) - else: - (z_EKF, P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py deleted file mode 100755 index b644d477..00000000 --- a/workspace/src/barc/src/state_estimation_DynBkMdl_mixed.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -from Localization_helpers import Localization -from barc.msg import ECU, pos_info, Vel_est -from sensor_msgs.msg import Imu -from marvelmind_nav.msg import hedge_pos -from std_msgs.msg import Header -from numpy import cos, sin, eye, array, zeros, diag, arctan, tan, unwrap -from observers import ekf -from system_models import f_KinBkMdl_mixed, h_KinBkMdl_mixed -from tf import transformations - -# ***_meas are values that are used by the Kalman filters -# ***_raw are raw values coming from the sensors - -class StateEst(object): - """This class contains all variables that are read from the sensors and then passed to - the Kalman filter.""" - # input variables - cmd_servo = 0 - cmd_motor = 0 - - # IMU - yaw_prev = 0 - yaw0 = 0 # yaw at t = 0 - yaw_meas = 0 - psiDot_meas = 0 - a_x_meas = 0 - a_y_meas = 0 - - # Velocity - vel_meas = 0 - - # GPS - x_meas = 0 - y_meas = 0 - - # General variables - t0 = 0 # Time when the estimator was started - running = False # bool if the car is driving - - def __init__(self): - self.x_meas = 0 - - # ecu command update - def ecu_callback(self, data): - self.cmd_motor = data.motor # input motor force [N] - self.cmd_servo = data.servo # input steering angle [rad] - if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero - self.running = True - - # ultrasound gps data - def gps_callback(self, data): - # units: [rad] and [rad/s] - #current_t = rospy.get_rostime().to_sec() - self.x_meas = data.x_m - self.y_meas = data.y_m - - # imu measurement update - def imu_callback(self, data): - # units: [rad] and [rad/s] - current_t = rospy.get_rostime().to_sec() - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) - # yaw_meas is element of [-pi,pi] - yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) - self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping - # from this point on 'yaw' will be definitely unwrapped (smooth)! - if not self.running: - self.yaw0 = yaw # set yaw0 to current yaw - self.yaw_meas = 0 # and current yaw to zero - else: - self.yaw_meas = yaw - self.yaw0 - - # extract angular velocity and linear acceleration data - #w_x = data.angular_velocity.x - #w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - #a_z = data.linear_acceleration.z - - self.psiDot_meas = w_z - self.a_x_meas = a_x - self.a_y_meas = a_y - - def vel_est_callback(self, data): - if not data.vel_est == self.vel_meas or not self.running: # if we're receiving a new measurement - self.vel_meas = data.vel_est - -# state estimation node -def state_estimation(): - se = StateEst() - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, se.imu_callback) - rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) - rospy.Subscriber('ecu', ECU, se.ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) - state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) - - # get vehicle dimension parameters - L_f = rospy.get_param("L_a") # distance from CoG to front axel - L_r = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_f, L_r) - - # get EKF observer properties - psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation - gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode - - # set node rate - loop_rate = 25 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - se.t0 = rospy.get_rostime().to_sec() - - z_EKF = zeros(11) # x, y, psi, v, psi_drift - P = eye(11) # initial dynamics coveriance matrix - - Q = diag([0.01,0.01,1/2*dt**2,1/2*dt**2,0.001,0.001,0.01,0.01,0.00001,0.00001,0.00001]) - R = diag([0.1,0.1,0.1,0.1,0.01,1.0,1.0]) - - # Set up track parameters - l = Localization() - l.create_track() - l.prepare_trajectory(0.06) - - d_f = 0 - - # Estimation variables - (x_est, y_est) = [0]*2 - - while not rospy.is_shutdown(): - # make R values dependent on current measurement (robust against outliers) - sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - if sq_gps_dist > 0.2: - R[0,0] = 1+10*sq_gps_dist**2 - R[1,1] = 1+10*sq_gps_dist**2 - else: - R[0,0] = 0.1 - R[1,1] = 0.1 - - # get measurement - y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas]) - - # define input - u = [0,0] - - # build extra arguments for non-linear function - args = (u, vhMdl, dt, est_mode) - - # apply EKF and get each state estimate - (z_EKF, P) = ekf(f_KinBkMdl_mixed, z_EKF, P, h_KinBkMdl_mixed, y, Q, R, args) - - # Read values - (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, a_x_drift_est, a_y_drift_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate - - # Update track position - l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) # v = v_x - l.find_s() - #l.s = 0 - #l.epsi = 0 - #l.s_start = 0 - - # and then publish position info - ros_t = rospy.get_rostime() - state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, l.v, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - l.coeffX.tolist(), l.coeffY.tolist(), - l.coeffTheta.tolist(), l.coeffCurvature.tolist())) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py deleted file mode 100755 index d53ed0f0..00000000 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ /dev/null @@ -1,244 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -import time -import os -from sensor_msgs.msg import Imu -from geometry_msgs.msg import Vector3 -from barc.msg import ECU, Encoder, Z_KinBkMdl -from numpy import pi, cos, sin, eye, array, zeros, unwrap -from observers import kinematicLuembergerObserver, ekf -from system_models import f_KinBkMdl, h_KinBkMdl, f_KinBkMdl_predictive, h_KinBkMdl_predictive -from tf import transformations -from numpy import unwrap, diag -from os.path import expanduser - -# input variables [default values] -d_f = 0 # steering angle [deg] -acc = 0 # acceleration [m/s] - -# raw measurement variables -yaw_prev = 0 -(roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) -yaw_prev = 0 -yaw_local = 0 -read_yaw0 = False -psi = 0 -psi_meas = 0 -x_meas = 0 -y_meas = 0 - -# from encoder -v = 0 -v_meas = 0 -t0 = time.time() -n_FL = 0 # counts in the front left tire -n_FR = 0 # counts in the front right tire -n_BL = 0 # counts in the back left tire -n_BR = 0 # counts in the back right tire -n_FL_prev = 0 -n_FR_prev = 0 -n_BL_prev = 0 -n_BR_prev = 0 -r_tire = 0.036 # radius from tire center to perimeter along magnets [m] -dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge [m] - - -# ecu command update -def ecu_callback(data): - global acc, d_f - acc = data.motor # input acceleration - d_f = data.servo # input steering angle - -# imu measurement update -def imu_callback(data): - # units: [rad] and [rad/s] - global roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z - global yaw_prev, yaw0, read_yaw0, yaw_local, psi_meas - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll, pitch, yaw) = transformations.euler_from_quaternion(quaternion) - - # save initial measurements - if not read_yaw0: - read_yaw0 = True - yaw_prev = yaw - yaw0 = yaw - - # unwrap measurement - yaw = unwrap(array([yaw_prev, yaw]), discont = pi)[1] - yaw_prev = yaw - yaw_local = yaw - yaw0 - psi_meas = yaw_local - - # extract angular velocity and linear acceleration data - w_x = data.angular_velocity.x - w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - a_z = data.linear_acceleration.z - -# ultrasound gps data -def gps_callback(data): - # units: [rad] and [rad/s] - global x_meas, y_meas - x_meas = data.x/100 # data is given in cm - y_meas = data.y/100 - -# encoder measurement update -def enc_callback(data): - global v, t0, dt_v_enc, v_meas - global n_FL, n_FR, n_FL_prev, n_FR_prev - global n_BL, n_BR, n_BL_prev, n_BR_prev - - n_FL = data.FL - n_FR = data.FR - n_BL = data.BL - n_BR = data.BR - - # compute time elapsed - tf = time.time() - dt = tf - t0 - - # if enough time elapse has elapsed, estimate v_x - if dt >= dt_v_enc: - # compute speed : speed = distance / time - v_FL = float(n_FL - n_FL_prev)*dx_qrt/dt - v_FR = float(n_FR - n_FR_prev)*dx_qrt/dt - v_BL = float(n_BL - n_BL_prev)*dx_qrt/dt - v_BR = float(n_BR - n_BR_prev)*dx_qrt/dt - - # Uncomment/modify according to your encoder setup - # v_meas = (v_FL + v_FR)/2.0 - # Modification for 3 working encoders - v_meas = (v_FL + v_BL + v_BR)/2.0 - # Modification for bench testing (driven wheels only) - # v = (v_BL + v_BR)/2.0 - - # update old data - n_FL_prev = n_FL - n_FR_prev = n_FR - n_BL_prev = n_BL - n_BR_prev = n_BR - t0 = tf - -# state estimation node -def state_estimation(): - global dt_v_enc - global v_meas, psi_meas - global est_mode - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, imu_callback) - rospy.Subscriber('encoder', Encoder, enc_callback) - rospy.Subscriber('ecu', ECU, ecu_callback) - rospy.Subscriber('indoor_gps', Vector3, gps_callback) - state_pub = rospy.Publisher('state_estimate', Z_KinBkMdl, queue_size = 10) - - # get vehicle dimension parameters - L_a = rospy.get_param("L_a") # distance from CoG to front axel - L_b = rospy.get_param("L_b") # distance from CoG to rear axel - est_mode = rospy.get_param("est_mode") # estimation mode - # mode 1: gps, IMU for orientation and encoders for v - # mode 2: no gps, IMU for orientation and encoders for v, x, y - # mode 3: only pgps, no IMU, no encoders (experimental) - vhMdl = (L_a, L_b) - - # get encoder parameters - dt_v_enc = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x from encoders - - # get EKF observer properties - q_std = rospy.get_param("state_estimation/q_std") # std of process noise - psi_std = rospy.get_param("state_estimation/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation/v_std") - gps_std = rospy.get_param("state_estimation/gps_std") # std of gps measurements - - t = time.localtime(time.time()) - # file = open("/home/felix/rosbag/info_%i_%02i_%02i_%02i_%02i_%02i.txt"%(t.tm_year,t.tm_mon,t.tm_mday,t.tm_hour,t.tm_min,t.tm_sec),'w') - # file.write("EKF parameters\n===============\n") - # file.write("q = %f\ngps_std = %f\npsi_std = %f\nv_std = %f"%(q_std,gps_std,psi_std,v_std)) - # file.close() - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - t0 = time.time() - - # estimation variables for Luemberger observer - #z_EKF = zeros(4) - z_EKF = zeros(8) - - # estimation variables for EKF - P = eye(8) # initial dynamics coveriance matrix - Q = (q_std**2)*eye(4) # process noise coveriance matrix - Q_predictive= (q_std**2)*eye(8) - # R = (r_std**2)*eye(4) # measurement noise coveriance matrix - if est_mode==1: - R = diag([gps_std,gps_std,psi_std,v_std])**2 - elif est_mode==2: - R = diag([psi_std,v_std])**2 - elif est_mode==3: - R = (gps_std**2)*eye(2) - else: - rospy.logerr("No estimation mode selected.") - - - # start loop - while not rospy.is_shutdown(): - - # collect inputs - u = array([ d_f, acc ]) - args = (u,vhMdl,dt) - - # publish state estimate - #(x_e, y_e, psi_e, v_e) = z_EKF - (x_e, y_e, psi_e, v_e, x_e_pred, y_e_pred, psi_e_pred, v_e_pred) = z_EKF - - # predict state in 0.1 seconds - #z_pred = f_KinBkMdl(z_EKF,u,vhMdl,0.1) - - # publish information - #state_pub.publish( Z_KinBkMdl(x_e, y_e, psi_e, v_e) ) - #state_pub.publish( Z_KinBkMdl(z_pred[0],z_pred[1],z_pred[2],z_pred[3])) - state_pub.publish( Z_KinBkMdl( x_e_pred, y_e_pred, psi_e_pred, v_e_pred )) - - # collect measurements, inputs, system properties - - if est_mode==1: - y = array([x_meas, y_meas, psi_meas, v_meas]) - elif est_mode==2: - y = array([psi_meas, v_meas]) - elif est_mode==3: - y = array([x_meas, y_meas]) - - # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_KinBkMdl_predictive, z_EKF, P, h_KinBkMdl_predictive, y, Q_predictive, R, args ) - - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py b/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py deleted file mode 100755 index aec42847..00000000 --- a/workspace/src/barc/src/state_estimation_KinBkMdl_mixed.py +++ /dev/null @@ -1,248 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -from Localization_helpers import Localization -from barc.msg import ECU, pos_info, Vel_est -from sensor_msgs.msg import Imu -from marvelmind_nav.msg import hedge_pos -from std_msgs.msg import Header -from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin -from observers import ekf -from system_models import f_KinBkMdl_2, h_KinBkMdl_2 -from tf import transformations -import math - -# ***_meas are values that are used by the Kalman filters -# ***_raw are raw values coming from the sensors - -class StateEst(object): - """This class contains all variables that are read from the sensors and then passed to - the Kalman filter.""" - # input variables - cmd_servo = 0 - cmd_motor = 0 - cmd_t = 0 - - # IMU - yaw_prev = 0 - yaw0 = 0 # yaw at t = 0 - yaw_meas = 0 - psiDot_meas = 0 - a_x_meas = 0 - a_y_meas = 0 - imu_updated = False - - # Velocity - vel_meas = 0 - vel_updated = False - vel_prev = 0 - vel_count = 0 # this counts how often the same vel measurement has been received - - # GPS - x_meas = 0 - y_meas = 0 - gps_updated = False - - # General variables - t0 = 0 # Time when the estimator was started - running = False # bool if the car is driving - - def __init__(self): - self.x_meas = 0 - - # ecu command update - def ecu_callback(self, data): - self.cmd_motor = data.motor # input motor force [N] - self.cmd_servo = data.servo # input steering angle [rad] - if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero - self.running = True - - # ultrasound gps data - def gps_callback(self, data): - # units: [rad] and [rad/s] - #current_t = rospy.get_rostime().to_sec() - self.x_meas = data.x_m - self.y_meas = data.y_m - self.gps_updated = True - - # imu measurement update - def imu_callback(self, data): - # units: [rad] and [rad/s] - current_t = rospy.get_rostime().to_sec() - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) - # yaw_meas is element of [-pi,pi] - yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) - self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping - # from this point on 'yaw' will be definitely unwrapped (smooth)! - if not self.running: - self.yaw0 = yaw # set yaw0 to current yaw - self.yaw_meas = 0 # and current yaw to zero - else: - self.yaw_meas = yaw - self.yaw0 - - # extract angular velocity and linear acceleration data - #w_x = data.angular_velocity.x - #w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - #a_z = data.linear_acceleration.z - - self.psiDot_meas = w_z - self.a_x_meas = a_x - self.a_y_meas = a_y - self.imu_updated = True - - def vel_est_callback(self, data): - #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est - if data.vel_est != self.vel_prev: - self.vel_meas = data.vel_est - self.vel_updated = True - self.vel_prev = data.vel_est - self.vel_count = 0 - else: - self.vel_count = self.vel_count + 1 - if self.vel_count > 10: # if 10 times in a row the same measurement - self.vel_meas = 0 # set velocity measurement to zero - self.vel_updated = True - -# state estimation node -def state_estimation(): - se = StateEst() - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, se.imu_callback) - rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) - rospy.Subscriber('ecu', ECU, se.ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) - state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) - - # get vehicle dimension parameters - L_f = rospy.get_param("L_a") # distance from CoG to front axel - L_r = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_f, L_r) - - # get EKF observer properties - psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation - gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - se.t0 = rospy.get_rostime().to_sec() - - z_EKF = zeros(6) # x, y, psi, v, psi_drift - P = eye(6) # initial dynamics coveriance matrix - - Q = diag([0.1,0.1,1.0,0.1,0.1,0.01]) - R = diag([1.0,1.0,1.0,1.0,5.0]) - - # Set up track parameters - l = Localization() - l.create_track() - l.prepare_trajectory(0.06) - - d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag - d_f_lp = 0 - a_lp = 0 - - # Estimation variables - (x_est, y_est) = [0]*2 - bta = 0 - v_est = 0 - psi_est = 0 - - while not rospy.is_shutdown(): - # make R values dependent on current measurement (robust against outliers) - sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - if se.gps_updated and sq_gps_dist < 0.5: # if there's a new gps value: - R[0,0] = 10.0 - R[1,1] = 10.0 - else: - # otherwise just extrapolate measurements: - se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) - se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) - R[0,0] = 100.0 - R[1,1] = 100.0 - # if se.imu_updated: - # R[3,3] = 1.0 - # R[4,4] = 5.0 - # else: - # R[3,3] = 10.0 - # R[4,4] = 50.0 - # if se.vel_updated: - # R[2,2] = 0.1 - # else: - # R[2,2] = 1.0 - - se.gps_updated = False - se.imu_updated = False - se.vel_updated = False - # get measurement - y = array([se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas]) - - # define input - #d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering - d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering - a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) - #u = [se.cmd_motor, d_f_hist.pop(0)] - u = [a_lp, d_f_lp] - - # build extra arguments for non-linear function - args = (u, vhMdl, dt, est_mode) - - # apply EKF and get each state estimate - (z_EKF, P) = ekf(f_KinBkMdl_2, z_EKF, P, h_KinBkMdl_2, y, Q, R, args) - - # Read values - (x_est, y_est, psi_est, v_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate - bta = math.atan2(L_f*tan(u[1]), L_f+L_r) - v_y_est = L_r*psi_dot_est # problem: results in linear dependency in system ID - v_y_est = sin(bta)*v_est - v_x_est = cos(bta)*v_est - - # Update track position - l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) - l.find_s() - #l.s = 0 - #l.epsi = 0 - #l.s_start = 0 - - # and then publish position info - ros_t = rospy.get_rostime() - state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - 0, 0, l.coeffX.tolist(), l.coeffY.tolist(), - l.coeffTheta.tolist(), l.coeffCurvature.tolist())) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py deleted file mode 100755 index a9e6aad4..00000000 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel_sim.py +++ /dev/null @@ -1,299 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -from Localization_helpers import Localization -from barc.msg import ECU, pos_info, Vel_est -from sensor_msgs.msg import Imu -from marvelmind_nav.msg import hedge_pos -from std_msgs.msg import Header -from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append -from numpy import ones, polyval, delete, size -from observers import ekf -from system_models import f_SensorKinematicModel, h_SensorKinematicModel -from tf import transformations -import math - -# ***_meas are values that are used by the Kalman filters -# ***_raw are raw values coming from the sensors - -class StateEst(object): - """This class contains all variables that are read from the sensors and then passed to - the Kalman filter.""" - # input variables - cmd_servo = 0 - cmd_motor = 0 - cmd_t = 0 - - # IMU - yaw_prev = 0 - yaw0 = 0 # yaw at t = 0 - yaw_meas = 0 - psiDot_meas = 0 - a_x_meas = 0 - a_y_meas = 0 - imu_updated = False - att = (0,0,0) # attitude - - # Velocity - vel_meas = 0 - vel_updated = False - vel_prev = 0 - vel_count = 0 # this counts how often the same vel measurement has been received - - # GPS - x_meas = 0 - y_meas = 0 - gps_updated = False - x_hist = zeros(15) - y_hist = zeros(15) - t_gps = zeros(15) - c_X = array([0,0,0]) - c_Y = array([0,0,0]) - - x_true = 0 - y_true = 0 - v_x_true = 0 - v_y_true = 0 - psi_true = 0 - psiDot_true = 0 - - # General variables - t0 = 0 # Time when the estimator was started - running = False # bool if the car is driving - - def __init__(self): - self.x_meas = 0 - - # ecu command update - def ecu_callback(self, data): - self.cmd_motor = data.motor # input motor force [N] - self.cmd_servo = data.servo # input steering angle [rad] - if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero - self.running = True - - # ultrasound gps data - def gps_callback(self, data): - """This function is called when a new GPS signal is received.""" - # units: [rad] and [rad/s] - t_now = rospy.get_rostime().to_sec()-self.t0 - t_msg = data.header.stamp.to_sec()-self.t0 - #if abs(t_now - t_msg) > 0.1: - # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) - self.x_meas = data.x_m - self.y_meas = data.y_m - self.x_hist = append(self.x_hist, data.x_m) - self.y_hist = append(self.y_hist, data.y_m) - self.t_gps = append(self.t_gps, t_now) - #self.x_hist = delete(self.x_hist,0) - #self.y_hist = delete(self.y_hist,0) - #self.t_gps = delete(self.t_gps,0) - self.x_hist = self.x_hist[self.t_gps > t_now-1.0] - self.y_hist = self.y_hist[self.t_gps > t_now-1.0] - self.t_gps = self.t_gps[self.t_gps > t_now-1.0] - sz = size(self.t_gps, 0) - if sz > 0: - t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T - self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] - self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] - self.gps_updated = True - - def real_val(self, data): - self.x_true = data.x - self.y_true = data.y - self.v_x_true = data.v_x - self.v_y_true = data.v_y - self.psi_true = data.psi - self.psiDot_true = data.psiDot - - # imu measurement update - def imu_callback(self, data): - # units: [rad] and [rad/s] - current_t = rospy.get_rostime().to_sec() - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) - # yaw_meas is element of [-pi,pi] - yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) - self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping - # from this point on 'yaw' will be definitely unwrapped (smooth)! - if not self.running: - self.yaw0 = yaw # set yaw0 to current yaw - self.yaw_meas = 0 # and current yaw to zero - else: - self.yaw_meas = yaw - self.yaw0 - - # extract angular velocity and linear acceleration data - #w_x = data.angular_velocity.x - #w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - a_z = data.linear_acceleration.z - - self.psiDot_meas = w_z - # The next two lines 'project' the measured linear accelerations to a horizontal plane - #self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z - #self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z - #print "Pitch: %f"%(pitch_raw) - # print "Roll: %f"%(roll_raw) - self.a_x_meas = a_x - self.a_y_meas = a_y - self.att = (roll_raw,pitch_raw,yaw_raw) - self.imu_updated = True - - def vel_est_callback(self, data): - #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est - if data.vel_est != self.vel_prev: - self.vel_meas = data.vel_est - self.vel_updated = True - self.vel_prev = data.vel_est - self.vel_count = 0 - else: - self.vel_count = self.vel_count + 1 - if self.vel_count > 10: # if 10 times in a row the same measurement - self.vel_meas = 0 # set velocity measurement to zero - self.vel_updated = True - -# state estimation node -def state_estimation(): - se = StateEst() - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, se.imu_callback) - rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) - rospy.Subscriber('ecu', ECU, se.ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) - rospy.Subscriber('real_val', pos_info, se.real_val) - state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=50) - - # get vehicle dimension parameters - L_f = rospy.get_param("L_a") # distance from CoG to front axel - L_r = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_f, L_r) - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - se.t0 = rospy.get_rostime().to_sec() # set initial time - - z_EKF = zeros(14) # x, y, psi, v, psi_drift - P = eye(14) # initial dynamics coveriance matrix - - qa = 10 - qp = 50 - # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v - Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) - Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) - R = diag([0.5,0.5,0.5,0.1,10.0,10.0,10.0, 0.5,0.5,0.1,0.5]) - R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 0.5,0.5,0.1,0.5]) - # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v - - # Set up track parameters - l = Localization() - l.create_track() - l.prepare_trajectory(0.06) - - d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag - d_f_lp = 0 - a_lp = 0 - - t_now = 0 - - # Estimation variables - (x_est, y_est, a_x_est, a_y_est) = [0]*4 - bta = 0 - v_est = 0 - psi_est = 0 - - while not rospy.is_shutdown(): - t_now = rospy.get_rostime().to_sec()-se.t0 - # make R values dependent on current measurement (robust against outliers) - sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - # if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: - # R[0,0] = 1.0 - # R[1,1] = 1.0 - # else: - # # otherwise just extrapolate measurements: - # #se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) - # #se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) - # R[0,0] = 10.0 - # R[1,1] = 10.0 - # if se.imu_updated: - # R[3,3] = 1.0 - # R[4,4] = 5.0 - # else: - # R[3,3] = 10.0 - # R[4,4] = 50.0 - if se.vel_updated: - R[2,2] = 0.1 - R[10,10] = 0.1 - else: - R[2,2] = 1.0 - R[10,10] = 0.1 - se.x_meas = polyval(se.c_X,t_now) - se.y_meas = polyval(se.c_Y,t_now) - se.gps_updated = False - se.imu_updated = False - se.vel_updated = False - # get measurement - y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, - se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas]) - - # define input - d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering - #d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering - a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) - u = [a_lp, d_f_hist.pop(0)] - #u = [a_lp, d_f_lp] - - # build extra arguments for non-linear function - args = (u, vhMdl, dt, 0) - - # apply EKF and get each state estimate - (z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args) - # Read values - (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, - x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate - - # Update track position - #l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) - l.set_pos(se.x_true, se.y_true, se.psi_true, v_x_est, v_y_est, psi_dot_est) - l.find_s() - #l.s = 0 - #l.epsi = 0 - #l.s_start = 0 - - # and then publish position info - ros_t = rospy.get_rostime() - state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, se.x_true, se.y_true, se.v_x_true, se.v_y_true, - se.psi_true, se.psiDot_true, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, (0,), (0,), - (0,), l.coeffCurvature.tolist())) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_SensorModel.py b/workspace/src/barc/src/state_estimation_SensorModel.py deleted file mode 100755 index 4900ec13..00000000 --- a/workspace/src/barc/src/state_estimation_SensorModel.py +++ /dev/null @@ -1,254 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -from Localization_helpers import Localization -from barc.msg import ECU, pos_info, Vel_est -from sensor_msgs.msg import Imu -from marvelmind_nav.msg import hedge_pos -from std_msgs.msg import Header -from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin -from observers import ekf -from system_models import f_SensorModel, h_SensorModel, f_KinBkMdl_2, h_KinBkMdl_2 -from tf import transformations -import math - -# ***_meas are values that are used by the Kalman filters -# ***_raw are raw values coming from the sensors - -class StateEst(object): - """This class contains all variables that are read from the sensors and then passed to - the Kalman filter.""" - # input variables - cmd_servo = 0 - cmd_motor = 0 - cmd_t = 0 - - # IMU - yaw_prev = 0 - yaw0 = 0 # yaw at t = 0 - yaw_meas = 0 - psiDot_meas = 0 - a_x_meas = 0 - a_y_meas = 0 - imu_updated = False - att = (0,0,0) # attitude - - # Velocity - vel_meas = 0 - vel_updated = False - vel_prev = 0 - vel_count = 0 # this counts how often the same vel measurement has been received - - # GPS - x_meas = 0 - y_meas = 0 - gps_updated = False - - # General variables - t0 = 0 # Time when the estimator was started - running = False # bool if the car is driving - - def __init__(self): - self.x_meas = 0 - - # ecu command update - def ecu_callback(self, data): - self.cmd_motor = data.motor # input motor force [N] - self.cmd_servo = data.servo # input steering angle [rad] - if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero - self.running = True - - # ultrasound gps data - def gps_callback(self, data): - # units: [rad] and [rad/s] - #current_t = rospy.get_rostime().to_sec() - self.x_meas = data.x_m - self.y_meas = data.y_m - self.gps_updated = True - - # imu measurement update - def imu_callback(self, data): - # units: [rad] and [rad/s] - current_t = rospy.get_rostime().to_sec() - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) - # yaw_meas is element of [-pi,pi] - yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) - self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping - # from this point on 'yaw' will be definitely unwrapped (smooth)! - if not self.running: - self.yaw0 = yaw # set yaw0 to current yaw - self.yaw_meas = 0 # and current yaw to zero - else: - self.yaw_meas = yaw - self.yaw0 - - # extract angular velocity and linear acceleration data - #w_x = data.angular_velocity.x - #w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - a_z = data.linear_acceleration.z - - self.psiDot_meas = w_z - # The next two lines 'project' the measured linear accelerations to a horizontal plane - self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z - self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z - #print "Pitch: %f"%(pitch_raw) - # print "Roll: %f"%(roll_raw) - #self.a_x_meas = a_x - #self.a_y_meas = a_y - self.att = (roll_raw,pitch_raw,yaw_raw) - self.imu_updated = True - - def vel_est_callback(self, data): - #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est - if data.vel_est != self.vel_prev: - self.vel_meas = data.vel_est - self.vel_updated = True - self.vel_prev = data.vel_est - self.vel_count = 0 - else: - self.vel_count = self.vel_count + 1 - if self.vel_count > 10: # if 10 times in a row the same measurement - self.vel_meas = 0 # set velocity measurement to zero - self.vel_updated = True - -# state estimation node -def state_estimation(): - se = StateEst() - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, se.imu_callback) - rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) - rospy.Subscriber('ecu', ECU, se.ecu_callback) - rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback) - state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) - - # get vehicle dimension parameters - L_f = rospy.get_param("L_a") # distance from CoG to front axel - L_r = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_f, L_r) - - # get EKF observer properties - psi_std = rospy.get_param("state_estimation_dynamic/psi_std") # std of measurementnoise - v_std = rospy.get_param("state_estimation_dynamic/v_std") # std of velocity estimation - gps_std = rospy.get_param("state_estimation_dynamic/gps_std") # std of gps measurements - est_mode = rospy.get_param("state_estimation_dynamic/est_mode") # estimation mode - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - se.t0 = rospy.get_rostime().to_sec() - - z_EKF = zeros(9) # x, y, psi, v, psi_drift - P = eye(9) # initial dynamics coveriance matrix - - #Q = diag([0.01,0.01,0.1,0.1,1.0,1.0,0.01,0.1,0.01]) - #R = diag([1.0,1.0,1.0,0.1,1.0,1.0,1.0]) - # x, y, vx, vy, ax, ay, psi, psidot, psidrift - Q = diag([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - R = diag([1.0,1.0,1.0,0.1,10.0,100.0,100.0]) - # x,y,v,psi,psiDot,a_x,a_y - - # Set up track parameters - l = Localization() - l.create_track() - l.prepare_trajectory(0.06) - - d_f_hist = [0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag - d_f_lp = 0 - a_lp = 0 - - # Estimation variables - (x_est, y_est, a_x_est, a_y_est) = [0]*4 - bta = 0 - v_est = 0 - psi_est = 0 - - while not rospy.is_shutdown(): - # make R values dependent on current measurement (robust against outliers) - sq_gps_dist = (se.x_meas-x_est)**2 + (se.y_meas-y_est)**2 - if se.gps_updated and sq_gps_dist < 0.8: # if there's a new gps value: - R[0,0] = 1.0 - R[1,1] = 1.0 - else: - # otherwise just extrapolate measurements: - #se.x_meas = x_est + dt*(v_est*cos(psi_est+bta)) - #se.y_meas = y_est + dt*(v_est*sin(psi_est+bta)) - R[0,0] = 10.0 - R[1,1] = 10.0 - # if se.imu_updated: - # R[3,3] = 1.0 - # R[4,4] = 5.0 - # else: - # R[3,3] = 10.0 - # R[4,4] = 50.0 - # if se.vel_updated: - # R[2,2] = 0.1 - # else: - # R[2,2] = 1.0 - - se.gps_updated = False - se.imu_updated = False - se.vel_updated = False - # get measurement - y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas]) - - # define input - #d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering - d_f_lp = d_f_lp + 0.2*(se.cmd_servo-d_f_lp) # low pass filter on steering - a_lp = a_lp + 0.5*(se.cmd_motor-a_lp) - #u = [se.cmd_motor, d_f_hist.pop(0)] - u = [a_lp, d_f_lp] - - # build extra arguments for non-linear function - args = (u, vhMdl, dt, est_mode) - - # apply EKF and get each state estimate - (z_EKF, P) = ekf(f_SensorModel, z_EKF, P, h_SensorModel, y, Q, R, args) - # Read values - (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate - - # Update track position - l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) - l.find_s() - #l.s = 0 - #l.epsi = 0 - #l.s_start = 0 - - # and then publish position info - ros_t = rospy.get_rostime() - state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est, l.s_start, l.x, l.y, l.v_x, l.v_y, - l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, psi_drift_est, - a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, (0,), (0,), - (0,), l.coeffCurvature.tolist())) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py index e4944e76..0cc38f29 100755 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -23,6 +23,8 @@ def f_KinBkMdl(z, u, vhMdl, dt, est_mode): process model input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] output: state at next time step z[k+1] + Does not account for drift in psi estimation! + -> Either put low trust on measured psi-values or add extra state for drift estimation! """ #c = array([0.5431, 1.2767, 2.1516, -2.4169]) @@ -49,50 +51,28 @@ def f_KinBkMdl(z, u, vhMdl, dt, est_mode): return array([x_next, y_next, psi_next, v_next]) -def f_KinBkMdl_mixed(z, u, vhMdl, dt, est_mode): - (l_A,l_B) = vhMdl - #bta = atan(l_A/(l_A+l_B)*tan(u[2])) - bta = math.atan2(z[3],z[2]) - zNext = [0]*11 - zNext[0] = z[0] + dt*(sqrt(z[2]**2+z[3]**2)*cos(z[6] + bta)) # x - zNext[1] = z[1] + dt*(sqrt(z[2]**2+z[3]**2)*sin(z[6] + bta)) # y - zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x - zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y - zNext[4] = z[4] # a_x - zNext[5] = z[5] # a_y - zNext[6] = z[6] + dt*(sqrt(z[2]**2+z[3]**2)/l_B*sin(bta)) # psi - zNext[7] = z[7] # psidot - zNext[8] = z[8] # drift_a_x - zNext[9] = z[9] # drift_a_y - zNext[10] = z[10] # drift_psi - return array(zNext) - -def f_SensorModel(z, u, vhMdl, dt, est_mode): - """ This Sensor model uses *only* sensor data and does not consider car dynamics! - Essentially, it only integrates acceleration data and puts it together with GPS data. """ - zNext = [0]*9 - zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x - zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y - zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x - zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y - zNext[4] = z[4] # a_x - zNext[5] = z[5] # a_y - zNext[6] = z[6] + dt*(z[7]) # psi - zNext[7] = z[7] # psidot - zNext[8] = z[8] # drift_psi - return array(zNext) - -def h_SensorModel(x, u, vhMdl, dt, est_mode): - """ This is the measurement model to the sensor model above """ - y = [0]*7 - y[0] = x[0] # x - y[1] = x[1] # y - y[2] = sqrt(x[2]**2+x[3]**2) # v - y[3] = x[6]+x[8] # psi - y[4] = x[7] # psiDot - y[5] = x[4] # a_x - y[6] = x[5] # a_y - return array(y) +def h_KinBkMdl(x, u, vhMdl, dt, est_mode): + """ + Measurement model + """ + if est_mode==1: # GPS, IMU, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==2: # IMU, Enc + C = array([[0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==3: # GPS + C = array([[1, 0, 0, 0], + [0, 1, 0, 0]]) + elif est_mode==4: # GPS, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 0, 1]]) + else: + print("Wrong est_mode") + return dot(C, x) def f_SensorKinematicModel(z, u, vhMdl, dt, est_mode): """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" @@ -132,114 +112,3 @@ def h_SensorKinematicModel(x, u, vhMdl, dt, est_mode): y[11] = x[2] # v_x y[12] = x[3] # v_y return array(y) - -def f_KinBkMdl_2(z, u, vhMdl, dt, est_mode): - (l_A,l_B) = vhMdl - #bta = atan(l_A/(l_A+l_B)*tan(u[2])) - bta = math.atan2(l_A*tan(u[1]),l_A+l_B) - #print "========================" - #print "bta = %f"%bta - #print "psi = %f"%z[6] - zNext = [0]*6 - zNext[0] = z[0] + dt*(z[3]*cos(z[2] + bta)) # x - zNext[1] = z[1] + dt*(z[3]*sin(z[2] + bta)) # y - zNext[2] = z[2] + dt*(z[3]/l_B*sin(bta)) # psi - zNext[3] = z[3] + dt*(u[0] - 0.5*z[3]) # v - zNext[4] = z[4] # psiDot - zNext[5] = z[5] # drift_psi - return array(zNext) - -def h_KinBkMdl_2(x, u, vhMdl, dt, est_mode): - """This Measurement model uses gps, imu and encoders""" - y = [0]*5 - y[0] = x[0] # x - y[1] = x[1] # y - y[2] = x[2]+x[5] # psi - y[3] = x[3] # v - y[4] = x[4] # psiDot - return array(y) - -def h_KinBkMdl_mixed(x, u, vhMdl, dt, est_mode): - """This Measurement model uses gps, imu and encoders""" - y = [0]*7 - y[0] = x[0] # x - y[1] = x[1] # y - y[2] = sqrt(x[2]**2+x[3]**2) # v - y[3] = x[6]+x[10] # psi - y[4] = x[7] # psiDot - y[5] = x[4]+x[8] # a_x - y[6] = x[5]+x[9] # a_y - return array(y) - -def h_KinBkMdl_mixed_nodrift(x, u, vhMdl, dt, est_mode): - """This Measurement model uses gps, imu and encoders""" - y = [0]*7 - y[0] = x[0] # x - y[1] = x[1] # y - y[2] = sqrt(x[2]**2+x[3]**2) # v - y[3] = x[6]+x[8] # psi - y[4] = x[7] # psiDot - y[5] = x[4] # a_x - y[6] = x[5] # a_y - return array(y) - -def h_KinBkMdl(x, u, vhMdl, dt, est_mode): - """ - Measurement model - """ - if est_mode==1: # GPS, IMU, Enc - C = array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]]) - elif est_mode==2: # IMU, Enc - C = array([[0, 0, 1, 0], - [0, 0, 0, 1]]) - elif est_mode==3: # GPS - C = array([[1, 0, 0, 0], - [0, 1, 0, 0]]) - elif est_mode==4: # GPS, Enc - C = array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 0, 1]]) - else: - print("Wrong est_mode") - return dot(C, x) - -def h_KinBkMdl_psi_drift(x, u, vhMdl, dt, est_mode): - """ - Measurement model - """ - if est_mode==1: # GPS, IMU, Enc - C = array([[1, 0, 0, 0, 0], - [0, 1, 0, 0, 0], - [0, 0, 1, 0, 1], - [0, 0, 0, 1, 0]]) - elif est_mode==2: # IMU, Enc - C = array([[0, 0, 1, 0, 1], - [0, 0, 0, 1, 0]]) - elif est_mode==3: # GPS - C = array([[1, 0, 0, 0, 0], - [0, 1, 0, 0, 0]]) - elif est_mode==4: # GPS, Enc - C = array([[1, 0, 0, 0, 0], - [0, 1, 0, 0, 0], - [0, 0, 0, 1, 0]]) - else: - print("Wrong est_mode") - return dot(C, x) - -def h_KinBkMdl_predictive(x): - """ - measurement model - """ - # For GPS, IMU and encoders: - # C = array([[1, 0, 0, 0], - # [0, 1, 0, 0], - # [0, 0, 1, 0], - # [0, 0, 0, 1]]) - # For GPS only: - C = array([[1, 0, 0, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0, 0, 0], - [0, 0, 1, 0, 0, 0, 0, 0]]) - return dot(C, x) diff --git a/workspace/src/barc/src/testCoeffConstr.jl b/workspace/src/barc/src/testCoeffConstr.jl deleted file mode 100644 index e20c5c2b..00000000 --- a/workspace/src/barc/src/testCoeffConstr.jl +++ /dev/null @@ -1,55 +0,0 @@ -using RobotOS -@rosimport barc.msg: ECU, pos_info -@rosimport data_service.msg: TimeData -@rosimport geometry_msgs.msg: Vector3 -rostypegen() -using barc.msg -using data_service.msg -using geometry_msgs.msg -using JuMP -using Ipopt -using JLD - -# log msg -include("barc_lib/classes.jl") -include("barc_lib/LMPC/functions.jl") -include("barc_lib/LMPC/MPC_models.jl") -include("barc_lib/LMPC/coeffConstraintCost.jl") -include("barc_lib/LMPC/solveMpcProblem.jl") -include("barc_lib/simModel.jl") - - -code = "794b" -log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" -#log_path_record = "$(homedir())/simulations/output-record-$(code).jld" -#d_rec = load(log_path_record) -d_lmpc = load(log_path_LMPC) - -buffersize = 2000 # size of oldTraj buffers - -# Define and initialize variables -# --------------------------------------------------------------- -# General LMPC variables -posInfo = PosInfo() -mpcCoeff = MpcCoeff() -lapStatus = LapStatus(1,1,false,false,0.3) -mpcSol = MpcSol() -trackCoeff = TrackCoeff() # info about track (at current position, approximated) -modelParams = ModelParams() -mpcParams = MpcParams() -mpcParams_pF = MpcParams() # for 1st lap (path following) - -InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) - -oldTraj = d_lmpc["oldTraj"] - - -posInfo.s = 5.0 -lapStatus.currentLap = 4 -oldTraj.count[3] = - -function testperf() - for i=1:100 - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) - end -end \ No newline at end of file From dd80add47a3950c94aa623a13dac8442f32965d0 Mon Sep 17 00:00:00 2001 From: maxb91 Date: Thu, 15 Dec 2016 18:53:45 +0100 Subject: [PATCH 108/183] Cleaned evaluation folder and added Readme. --- eval_sim/Kalman simulation/sim_kalman_imu.jl | 298 ++++++------ eval_sim/Kalman simulation/sim_kalman_imu2.jl | 353 -------------- .../Kalman simulation/sim_kalman_imu_findQ.jl | 361 -------------- eval_sim/Kalman simulation/sim_kalman_mix.jl | 386 --------------- .../Kalman simulation/sim_kalman_mix_2.jl | 412 ---------------- .../sim_kalman_mix_nodrift.jl | 443 ------------------ eval_sim/README.md | 78 +++ eval_sim/create_track_bezier2.py | 36 -- eval_sim/eval_data.jl | 68 +-- 9 files changed, 224 insertions(+), 2211 deletions(-) delete mode 100644 eval_sim/Kalman simulation/sim_kalman_imu2.jl delete mode 100644 eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl delete mode 100644 eval_sim/Kalman simulation/sim_kalman_mix.jl delete mode 100644 eval_sim/Kalman simulation/sim_kalman_mix_2.jl delete mode 100644 eval_sim/Kalman simulation/sim_kalman_mix_nodrift.jl create mode 100644 eval_sim/README.md delete mode 100644 eval_sim/create_track_bezier2.py diff --git a/eval_sim/Kalman simulation/sim_kalman_imu.jl b/eval_sim/Kalman simulation/sim_kalman_imu.jl index 75b1aefd..ec7a24f0 100644 --- a/eval_sim/Kalman simulation/sim_kalman_imu.jl +++ b/eval_sim/Kalman simulation/sim_kalman_imu.jl @@ -29,43 +29,59 @@ function main(code::AbstractString) y = zeros(sz,6) u = zeros(sz,2) - y_gps_imu = zeros(sz,7) + y_gps_imu = zeros(sz,13) + q = zeros(sz,2) P = zeros(7,7) x_est = zeros(length(t),7) - P_gps_imu = zeros(9,9,10000) - x_est_gps_imu = zeros(length(t),9) + P_gps_imu = zeros(14,14) + x_est_gps_imu = zeros(length(t),14) - yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end] gps_dist = zeros(length(t)) yaw_prev = yaw0 y_gps_imu[1,4] = 0 + qa = 1000 + qp = 1000 + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) # x, y, vx, vy, ax, ay, psi, psidot, psidrift - Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) - #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0]) - - Q_acc = [dt^5/20 0 dt^4/8 0 dt^3/6 0 0 0 0; - 0 dt^5/20 0 dt^4/8 0 dt^3/6 0 0 0; - dt^4/8 0 dt^3/3 0 dt^2/2 0 0 0 0; - 0 dt^4/8 0 dt^3/3 0 dt^2/2 0 0 0; - dt^3/6 0 dt^2/2 0 dt 0 0 0 0; - 0 dt^3/6 0 dt^2/2 0 dt 0 0 0; - 0 0 0 0 0 0 dt/10 0 0; - 0 0 0 0 0 0 0 dt 0; - 0 0 0 0 0 0 0 0 dt/100]*20 - - #Q_gps_imu = Q_acc - # x, y, v, psi, psidot, ax, ay + Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1]) + #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,10.0, 10.0,10.0]) + # x, y, v, psi, psidot, ax, a_y x, y, psi, v, v_x, v_y + + y_vel_prev = 0.0 + y_vel_est = 0.0 for i=2:length(t) # Collect measurements and inputs for this iteration + # Interpolate GPS-measurements: + x_p = gps_meas.z[(t[i].>gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1. 1 + c_x = [t_p.^2 t_p ones(sl,1)]\x_p + c_y = [t_p.^2 t_p ones(sl,1)]\y_p + x_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_x + y_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_y + else + x_int = 0 + y_int = 0 + end + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] - #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 + y_gps = [x_int y_int] + y_gps = pos_info.z[t[i].>pos_info.t,12:13][end,:] + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] @@ -74,166 +90,96 @@ function main(code::AbstractString) rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' - #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] - #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + a_y = imu_meas.z[t[i].>imu_meas.t,8][end] a_x = acc_f[1] a_y = acc_f[2] #y_yawdot = rot_f[3] + u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] + u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end] + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] - y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + if pos_info.z[t[i].>pos_info.t,15][end] != y_vel_prev + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + y_vel_prev = y_vel_est + else + y_vel_est = y_vel_est + dt*(u[i,1] - 0.5*x_est_gps_imu[i-1,13]) + end - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] - y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) - u[i,:] = cmd_log.z[t[i]-0.2.>(cmd_log.t),:][end,:] + bta = atan(0.5*tan(u[i,2])) + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10]) - # Adapt R-value of GPS according to distance to last point: - gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - if gps_dist[i] < 0.8 - R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 - R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 - else - R_gps_imu[1,1] = 10 - R_gps_imu[2,2] = 10 - end args = (u[i,:],dt,l_A,l_B) # Calculate new estimate - (x_est_gps_imu[i,:], P_gps_imu[:,:,i]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu[:,:,i-1],h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) end - #figure(5) - #plot(t-t0,y_gps_imu) - #grid("on") - println("Yaw0 = $yaw0") figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) - plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") + plot(t-t0,x_est_gps_imu[:,10:11],"-x") + #plot(pos_info.t-t0,pos_info.z[:,12:13],"-+") + #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") grid("on") - legend(["x_est","y_est","x_meas","y_meas"]) + legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) title("Comparison x,y estimate and measurement") figure(2) - plot(t-t0,x_est_gps_imu[:,[7,9]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) + plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") grid("on") - legend(["psi_est","psi_drift_est","psi_meas"]) + legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas","psi_onboard","psi_drift_onboard"]) figure(3) v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,[3:4,13]],"--",vel_est.t-t0,vel_est.z[:,1]) plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") - legend(["v_est","v_x_est","v_y_est","v_meas"]) + legend(["v_est","v_x_est","v_y_est","v_kin_est","v_meas","v_x_onboard","v_y_onboard"]) grid("on") title("Velocity estimate and measurement") figure(4) - plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) - #plot(pos_info.t-t0,pos_info.z[:,17:18]) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",pos_info.t-t0,pos_info.z[:,17:18],t-t0,x_est_gps_imu[:,5:6]) grid("on") - legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) + legend(["a_x_meas","a_y_meas","a_x_onboard","a_y_onboard","a_x_est","a_y_est",]) + # PLOT DIRECTIONS figure(5) - plot(x_est_gps_imu[:,1],x_est_gps_imu[:,2]) + plot(x_est_gps_imu[:,10],x_est_gps_imu[:,11]) grid("on") title("x-y-view") for i=1:10:size(pos_info.t,1) - #d_f = cmd_log.z[pos_info.t[i].>(cmd_log.t-0.2),2][end] - #bta = atan(l_A/(l_A+l_B)*tan(d_f)) dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] plot(lin[:,1],lin[:,2],"-+") end for i=1:10:size(t,1) bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) - dir = [cos(x_est_gps_imu[i,7]+bta) sin(x_est_gps_imu[i,7]+bta)] - lin = [x_est_gps_imu[i,1:2]; x_est_gps_imu[i,1:2] + 0.1*dir] + dir = [cos(x_est_gps_imu[i,12]+bta) sin(x_est_gps_imu[i,12]+bta)] # 7 = imu-yaw, 12 = model-yaw + lin = [x_est_gps_imu[i,10:11]; x_est_gps_imu[i,10:11] + 0.1*dir] plot(lin[:,1],lin[:,2],"-*") end - - # ax1=subplot(211) - # plot(t,y,"-x",t,x_est,"-*") - # grid("on") - # legend(["x","y","psi","v"]) - # subplot(212,sharex=ax1) - # plot(t,u) - # grid("on") - # legend(["a_x","d_f"]) - # figure(1) - # ax=subplot(5,1,1) - # for i=1:4 - # subplot(5,1,i,sharex=ax) - # plot(t,y[:,i],t,x_est[:,i],"-*") - # grid("on") - # end - # subplot(5,1,5,sharex=ax) - # plot(cmd_pwm_log.t,cmd_pwm_log.z) - # grid("on") - - - # figure(2) - # subplot(2,1,1) - # title("Comparison raw GPS data and estimate") - # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["x_m","x_est"]) - # grid("on") - # subplot(2,1,2) - # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["y_m","y_est"]) - # grid("on") - - # figure(3) - # plot(t,gps_dist) - # title("GPS distances") - # grid("on") - # # figure() - # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") - # # grid("on") - # # title("x-y-view") - # figure(4) - # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") - # title("Comparison simulation (-x) onboard-estimate (-*)") - # grid("on") - # legend(["x","y","psi","v","psi_drift"]) - - # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) - # figure(5) - # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) - # title("Comparison yaw") - # grid("on") + axis("equal") # figure(6) - # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + # plot(t-t0,q) # grid("on") - # title("Comparison of velocity measurement and estimate") - # legend(["estimate","measurement"]) - - #figure(7) - #plt[:hist](gps_dist,300) - #grid("on") + # title("Errors") + figure(7) + plot(imu_meas.t-t0,imu_meas.z[:,3],pos_info.t-t0,pos_info.z[:,11],t-t0,x_est_gps_imu[:,8]) + legend(["psiDot_meas","psiDot_onboard","psiDot_est"]) + grid("on") nothing end -function h(x,args) - C = [eye(6) zeros(6,1)] - C[4,4] = 1 - #C[3,3] = 0 - C[3,5] = 1 - C[5,5] = 0 - C[5,6] = 1 - C[6,6] = 0 - C[6,7] = 1 - return C*x -end function h_gps_imu(x,args) - y = zeros(7) + y = zeros(13) y[1] = x[1] # x y[2] = x[2] # y y[3] = sqrt(x[3]^2+x[4]^2) # v @@ -241,6 +187,12 @@ function h_gps_imu(x,args) y[5] = x[8] # psiDot y[6] = x[5] # a_x y[7] = x[6] # a_y + y[8] = x[10] # x + y[9] = x[11] # y + y[10] = x[12]+x[14] # psi + y[11] = x[13] # v + y[12] = x[3] # v_x + y[13] = x[4] # v_y return y end function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) @@ -251,30 +203,26 @@ function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) my_kp1 = h(mx_kp1,args) H = numerical_jac(h,mx_kp1,args) P12 = P_kp1*H' + q_GPS = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2] + q_GPS2 = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11] + # if q_GPS[1] > 0.005 + # R[1,1] = 100 + # R[2,2] = 100 + # else + # R[1,1] = 1 + # R[2,2] = 1 + # end + # if q_GPS2[1] > 0.005 + # R[8,8] = 100 + # R[9,9] = 100 + # else + # R[8,8] = 1 + # R[9,9] = 1 + # end K = P12*inv(H*P12+R) mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1 -end - -function simModel(z,args) - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - - zNext = copy(z) - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - #zNext[4] = z[4] + dt*(z[6]) # v - #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - zNext[5] = z[5] # psi_drift - zNext[6] = z[6] # a_x - zNext[7] = z[7] # a_y - return zNext + return mx_kp1, P_kp1, q_GPS[1], q_GPS2[1] end function simModel_gps_imu(z,args) @@ -289,10 +237,14 @@ function simModel_gps_imu(z,args) zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y zNext[5] = z[5] # a_x zNext[6] = z[6] # a_y - #zNext[7] = z[7] + dt*(z[3]/l_B*sin(bta)) # psi zNext[7] = z[7] + dt*z[8] # psi zNext[8] = z[8] # psidot zNext[9] = z[9] # drift_psi + zNext[10] = z[10] + dt*(z[13]*cos(z[12] + bta)) # x + zNext[11] = z[11] + dt*(z[13]*sin(z[12] + bta)) # y + zNext[12] = z[12] + dt*(z[13]/l_B*sin(bta)) # psi + zNext[13] = z[13] + dt*(u[1] - 0.5*z[13]) # v + zNext[14] = z[14] # psi_drift_2 return zNext end @@ -359,3 +311,43 @@ function rotMatrix(s::Char,deg::Float64) end return A end + +function plot_velocities(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + cmd_log = d_rec["cmd_log"] + + acc = diff(vel_est.z[:,1])./diff(vel_est.t_msg) + acc = smooth(acc,5) + t0 = vel_est.t[1] + ax1=subplot(211) + plot(vel_est.t-t0,vel_est.z) + plot(vel_est.t_msg-t0,vel_est.z,"--") + legend(["vel","fl","fr","bl","br"]) + grid("on") + xlabel("t [s]") + title("Velocity") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + plot(cmd_log.t_msg-t0,cmd_log.z[:,1],"--") + plot(vel_est.t_msg[1:end-1]-t0,acc) + grid("on") + legend(["u_a","u_a_sent","acceleration"]) + title("Commands") + xlabel("t [s]") + ylabel("") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_imu2.jl b/eval_sim/Kalman simulation/sim_kalman_imu2.jl deleted file mode 100644 index ec7a24f0..00000000 --- a/eval_sim/Kalman simulation/sim_kalman_imu2.jl +++ /dev/null @@ -1,353 +0,0 @@ -# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. -# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. - -using PyPlot -using JLD - - -function main(code::AbstractString) - #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - log_path_record = "$(homedir())/simulations/output-record-$(code).jld" - d_rec = load(log_path_record) - - imu_meas = d_rec["imu_meas"] - gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - #cmd_pwm_log = d_rec["cmd_pwm_log"] - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] - - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) - - l_A = 0.125 - l_B = 0.125 - - dt = 1/50 - t = t0:dt:t_end - sz = length(t) - y = zeros(sz,6) - u = zeros(sz,2) - - y_gps_imu = zeros(sz,13) - q = zeros(sz,2) - - P = zeros(7,7) - x_est = zeros(length(t),7) - P_gps_imu = zeros(14,14) - x_est_gps_imu = zeros(length(t),14) - - yaw0 = imu_meas.z[t0.>imu_meas.t,6][end] - gps_dist = zeros(length(t)) - - yaw_prev = yaw0 - y_gps_imu[1,4] = 0 - - qa = 1000 - qp = 1000 - - #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) - #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) - #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) - # x, y, vx, vy, ax, ay, psi, psidot, psidrift - Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1]) - #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) - R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,10.0, 10.0,10.0]) - # x, y, v, psi, psidot, ax, a_y x, y, psi, v, v_x, v_y - - y_vel_prev = 0.0 - y_vel_est = 0.0 - - for i=2:length(t) - # Collect measurements and inputs for this iteration - # Interpolate GPS-measurements: - x_p = gps_meas.z[(t[i].>gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1. 1 - c_x = [t_p.^2 t_p ones(sl,1)]\x_p - c_y = [t_p.^2 t_p ones(sl,1)]\y_p - x_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_x - y_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_y - else - x_int = 0 - y_int = 0 - end - - y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] - y_gps = [x_int y_int] - y_gps = pos_info.z[t[i].>pos_info.t,12:13][end,:] - - y_yaw = pos_info.z[t[i].>pos_info.t,14][end] - y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] - - att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] - acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] - rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] - acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' - #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' - a_x = imu_meas.z[t[i].>imu_meas.t,7][end] - a_y = imu_meas.z[t[i].>imu_meas.t,8][end] - a_x = acc_f[1] - a_y = acc_f[2] - #y_yawdot = rot_f[3] - - u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] - u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end] - - #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] - if pos_info.z[t[i].>pos_info.t,15][end] != y_vel_prev - y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] - y_vel_prev = y_vel_est - else - y_vel_est = y_vel_est + dt*(u[i,1] - 0.5*x_est_gps_imu[i-1,13]) - end - - - bta = atan(0.5*tan(u[i,2])) - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] - y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) - y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10]) - - - args = (u[i,:],dt,l_A,l_B) - - # Calculate new estimate - (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) - end - - println("Yaw0 = $yaw0") - figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") - plot(t-t0,x_est_gps_imu[:,10:11],"-x") - #plot(pos_info.t-t0,pos_info.z[:,12:13],"-+") - #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") - grid("on") - legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) - title("Comparison x,y estimate and measurement") - - figure(2) - plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) - plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") - grid("on") - legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas","psi_onboard","psi_drift_onboard"]) - - figure(3) - v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,[3:4,13]],"--",vel_est.t-t0,vel_est.z[:,1]) - plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") - legend(["v_est","v_x_est","v_y_est","v_kin_est","v_meas","v_x_onboard","v_y_onboard"]) - grid("on") - title("Velocity estimate and measurement") - - figure(4) - plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",pos_info.t-t0,pos_info.z[:,17:18],t-t0,x_est_gps_imu[:,5:6]) - grid("on") - legend(["a_x_meas","a_y_meas","a_x_onboard","a_y_onboard","a_x_est","a_y_est",]) - - # PLOT DIRECTIONS - figure(5) - plot(x_est_gps_imu[:,10],x_est_gps_imu[:,11]) - grid("on") - title("x-y-view") - for i=1:10:size(pos_info.t,1) - dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] - lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] - plot(lin[:,1],lin[:,2],"-+") - end - for i=1:10:size(t,1) - bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) - dir = [cos(x_est_gps_imu[i,12]+bta) sin(x_est_gps_imu[i,12]+bta)] # 7 = imu-yaw, 12 = model-yaw - lin = [x_est_gps_imu[i,10:11]; x_est_gps_imu[i,10:11] + 0.1*dir] - plot(lin[:,1],lin[:,2],"-*") - end - axis("equal") - - # figure(6) - # plot(t-t0,q) - # grid("on") - # title("Errors") - figure(7) - plot(imu_meas.t-t0,imu_meas.z[:,3],pos_info.t-t0,pos_info.z[:,11],t-t0,x_est_gps_imu[:,8]) - legend(["psiDot_meas","psiDot_onboard","psiDot_est"]) - grid("on") - nothing -end - -function h_gps_imu(x,args) - y = zeros(13) - y[1] = x[1] # x - y[2] = x[2] # y - y[3] = sqrt(x[3]^2+x[4]^2) # v - y[4] = x[7]+x[9] # psi - y[5] = x[8] # psiDot - y[6] = x[5] # a_x - y[7] = x[6] # a_y - y[8] = x[10] # x - y[9] = x[11] # y - y[10] = x[12]+x[14] # psi - y[11] = x[13] # v - y[12] = x[3] # v_x - y[13] = x[4] # v_y - return y -end -function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - q_GPS = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2] - q_GPS2 = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11] - # if q_GPS[1] > 0.005 - # R[1,1] = 100 - # R[2,2] = 100 - # else - # R[1,1] = 1 - # R[2,2] = 1 - # end - # if q_GPS2[1] > 0.005 - # R[8,8] = 100 - # R[9,9] = 100 - # else - # R[8,8] = 1 - # R[9,9] = 1 - # end - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1, q_GPS[1], q_GPS2[1] -end - -function simModel_gps_imu(z,args) - zNext = copy(z) - (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x - #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y - zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x - zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y - zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x - zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y - zNext[5] = z[5] # a_x - zNext[6] = z[6] # a_y - zNext[7] = z[7] + dt*z[8] # psi - zNext[8] = z[8] # psidot - zNext[9] = z[9] # drift_psi - zNext[10] = z[10] + dt*(z[13]*cos(z[12] + bta)) # x - zNext[11] = z[11] + dt*(z[13]*sin(z[12] + bta)) # y - zNext[12] = z[12] + dt*(z[13]/l_B*sin(bta)) # psi - zNext[13] = z[13] + dt*(u[1] - 0.5*z[13]) # v - zNext[14] = z[14] # psi_drift_2 - return zNext -end - -function numerical_jac(f,x,args) - y = f(x,args) - jac = zeros(size(y,1),size(x,1)) - eps = 1e-5 - xp = copy(x) - for i = 1:size(x,1) - xp[i] = x[i]+eps/2.0 - yhi = f(xp,args) - xp[i] = x[i]-eps/2.0 - ylo = f(xp,args) - xp[i] = x[i] - jac[:,i] = (yhi-ylo)/eps - end - return jac -end - -function initPlot() # Initialize Plots for export - linewidth = 0.4 - rc("axes", linewidth=linewidth) - rc("lines", linewidth=linewidth, markersize=2) - #rc("font", family="") - rc("axes", titlesize="small", labelsize="small") # can be set in Latex - rc("xtick", labelsize="x-small") - rc("xtick.major", width=linewidth/2) - rc("ytick", labelsize="x-small") - rc("ytick.major", width=linewidth/2) - rc("legend", fontsize="small") - rc("font",family="serif") - rc("font",size=8) - rc("figure",figsize=[4.5,3]) - #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") -end - -function unwrap!(p) - length(p) < 2 && return p - for i = 2:length(p) - d = p[i] - p[i-1] - if abs(d) > pi - p[i] -= floor((d+pi) / (2*pi)) * 2pi - end - end - return p -end - -function rotMatrix(s::Char,deg::Float64) - A = zeros(3,3) - if s=='x' - A = [1 0 0; - 0 cos(deg) sin(deg); - 0 -sin(deg) cos(deg)] - elseif s=='y' - A = [cos(deg) 0 -sin(deg); - 0 1 0; - sin(deg) 0 cos(deg)] - elseif s=='z' - A = [cos(deg) sin(deg) 0 - -sin(deg) cos(deg) 0 - 0 0 1] - else - warn("Wrong angle for rotation matrix") - end - return A -end - -function plot_velocities(code::AbstractString) - #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - log_path_record = "$(homedir())/simulations/output-record-$(code).jld" - d_rec = load(log_path_record) - - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] - cmd_log = d_rec["cmd_log"] - - acc = diff(vel_est.z[:,1])./diff(vel_est.t_msg) - acc = smooth(acc,5) - t0 = vel_est.t[1] - ax1=subplot(211) - plot(vel_est.t-t0,vel_est.z) - plot(vel_est.t_msg-t0,vel_est.z,"--") - legend(["vel","fl","fr","bl","br"]) - grid("on") - xlabel("t [s]") - title("Velocity") - subplot(212,sharex=ax1) - plot(cmd_log.t-t0,cmd_log.z[:,1]) - plot(cmd_log.t_msg-t0,cmd_log.z[:,1],"--") - plot(vel_est.t_msg[1:end-1]-t0,acc) - grid("on") - legend(["u_a","u_a_sent","acceleration"]) - title("Commands") - xlabel("t [s]") - ylabel("") -end - -function smooth(x,n) - y = zeros(size(x)) - for i=1:size(x,1) - start = max(1,i-n) - fin = min(size(x,1),start + 2*n) - y[i,:] = mean(x[start:fin,:],1) - end - return y -end \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl b/eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl deleted file mode 100644 index b7c32e74..00000000 --- a/eval_sim/Kalman simulation/sim_kalman_imu_findQ.jl +++ /dev/null @@ -1,361 +0,0 @@ -# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. -# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. - -using PyPlot -using JLD - - -function main(code::AbstractString) - #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - log_path_record = "$(homedir())/simulations/output-record-$(code).jld" - d_rec = load(log_path_record) - - imu_meas = d_rec["imu_meas"] - gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - #cmd_pwm_log = d_rec["cmd_pwm_log"] - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] - - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) - - l_A = 0.125 - l_B = 0.125 - - dt = 1/50 - t = t0:dt:t_end - sz = length(t) - y = zeros(sz,6) - u = zeros(sz,2) - - y_gps_imu = zeros(sz,7) - - P = zeros(7,7) - x_est = zeros(length(t),7) - P_gps_imu = zeros(9,9,10000) - A_gps_imu = zeros(9,9,10000) - x_est_gps_imu = zeros(length(t),9) - - yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]#imu_meas.z[1,6] - gps_dist = zeros(length(t)) - - yaw_prev = yaw0 - y_gps_imu[1,4] = 0 - - #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) - # x, y, vx, vy, ax, ay, psi, psidot, psidrift - Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) - #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) - R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0]) - - #Q_gps_imu = Q_acc - # x, y, v, psi, psidot, ax, ay - - for i=2:length(t) - # Collect measurements and inputs for this iteration - y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] - #y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 - y_yaw = pos_info.z[t[i].>pos_info.t,14][end] - y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] - - att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] - acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] - rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] - acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' - #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' - #a_x = imu_meas.z[t[i].>imu_meas.t,7][end] - #a_y = imu_meas.z[t[i].>imu_meas.t,8][end] - a_x = acc_f[1] - a_y = acc_f[2] - #y_yawdot = rot_f[3] - - #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] - y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] - - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] - y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) - - u[i,:] = cmd_log.z[t[i]-0.2.>(cmd_log.t),:][end,:] - - # Adapt R-value of GPS according to distance to last point: - gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - if gps_dist[i] < 0.8 - R_gps_imu[1,1] = 1#+100*gps_dist[i]^2 - R_gps_imu[2,2] = 1#+100*gps_dist[i]^2 - else - R_gps_imu[1,1] = 10 - R_gps_imu[2,2] = 10 - end - - args = (u[i,:],dt,l_A,l_B) - - # Calculate new estimate - (x_est_gps_imu[i,:], P_gps_imu[:,:,i], A_gps_imu[:,:,i]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu[:,:,i-1],h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) - end - - Q_max = zeros(9,9) - for i=1:length(t)-1 - Q_max = Q_max + x_est_gps_imu[i+1,:]'*x_est_gps_imu[i+1,:] - - x_est_gps_imu[i+1,:]'*x_est_gps_imu[i,:]*A_gps_imu[:,:,i]' - - A_gps_imu[:,:,i]*x_est_gps_imu[i,:]'*x_est_gps_imu[i+1,:] + - A_gps_imu[:,:,i]*x_est_gps_imu[i,:]'*x_est_gps_imu[i,:]*A_gps_imu[:,:,i]' - end - Q_max /= length(t) - - #figure(5) - #plot(t-t0,y_gps_imu) - #grid("on") - - println("Yaw0 = $yaw0") - figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z) - plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") - grid("on") - legend(["x_est","y_est","x_meas","y_meas"]) - title("Comparison x,y estimate and measurement") - - figure(2) - plot(t-t0,x_est_gps_imu[:,[7,9]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) - plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") - grid("on") - legend(["psi_est","psi_drift_est","psi_meas"]) - - figure(3) - v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z[:,1]) - plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") - legend(["v_est","v_x_est","v_y_est","v_meas"]) - grid("on") - title("Velocity estimate and measurement") - - figure(4) - plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) - #plot(pos_info.t-t0,pos_info.z[:,17:18]) - grid("on") - legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) - - figure(5) - plot(x_est_gps_imu[:,1],x_est_gps_imu[:,2]) - grid("on") - title("x-y-view") - for i=1:10:size(pos_info.t,1) - #d_f = cmd_log.z[pos_info.t[i].>(cmd_log.t-0.2),2][end] - #bta = atan(l_A/(l_A+l_B)*tan(d_f)) - dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] - lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] - plot(lin[:,1],lin[:,2],"-+") - end - for i=1:10:size(t,1) - bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) - dir = [cos(x_est_gps_imu[i,7]+bta) sin(x_est_gps_imu[i,7]+bta)] - lin = [x_est_gps_imu[i,1:2]; x_est_gps_imu[i,1:2] + 0.1*dir] - plot(lin[:,1],lin[:,2],"-*") - end - - # ax1=subplot(211) - # plot(t,y,"-x",t,x_est,"-*") - # grid("on") - # legend(["x","y","psi","v"]) - # subplot(212,sharex=ax1) - # plot(t,u) - # grid("on") - # legend(["a_x","d_f"]) - # figure(1) - # ax=subplot(5,1,1) - # for i=1:4 - # subplot(5,1,i,sharex=ax) - # plot(t,y[:,i],t,x_est[:,i],"-*") - # grid("on") - # end - # subplot(5,1,5,sharex=ax) - # plot(cmd_pwm_log.t,cmd_pwm_log.z) - # grid("on") - - - # figure(2) - # subplot(2,1,1) - # title("Comparison raw GPS data and estimate") - # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["x_m","x_est"]) - # grid("on") - # subplot(2,1,2) - # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["y_m","y_est"]) - # grid("on") - - # figure(3) - # plot(t,gps_dist) - # title("GPS distances") - # grid("on") - # # figure() - # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") - # # grid("on") - # # title("x-y-view") - # figure(4) - # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") - # title("Comparison simulation (-x) onboard-estimate (-*)") - # grid("on") - # legend(["x","y","psi","v","psi_drift"]) - - # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) - # figure(5) - # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) - # title("Comparison yaw") - # grid("on") - - # figure(6) - # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) - # grid("on") - # title("Comparison of velocity measurement and estimate") - # legend(["estimate","measurement"]) - - #figure(7) - #plt[:hist](gps_dist,300) - #grid("on") - nothing -end - -function h(x,args) - C = [eye(6) zeros(6,1)] - C[4,4] = 1 - #C[3,3] = 0 - C[3,5] = 1 - C[5,5] = 0 - C[5,6] = 1 - C[6,6] = 0 - C[6,7] = 1 - return C*x -end -function h_gps_imu(x,args) - y = zeros(7) - y[1] = x[1] # x - y[2] = x[2] # y - y[3] = sqrt(x[3]^2+x[4]^2) # v - y[4] = x[7]+x[9] # psi - y[5] = x[8] # psiDot - y[6] = x[5] # a_x - y[7] = x[6] # a_y - return y -end -function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1, A -end - -function simModel(z,args) - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - - zNext = copy(z) - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - #zNext[4] = z[4] + dt*(z[6]) # v - #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - zNext[5] = z[5] # psi_drift - zNext[6] = z[6] # a_x - zNext[7] = z[7] # a_y - return zNext -end - -function simModel_gps_imu(z,args) - zNext = copy(z) - (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x - #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y - zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x - zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y - zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x - zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y - zNext[5] = z[5] # a_x - zNext[6] = z[6] # a_y - #zNext[7] = z[7] + dt*(z[3]/l_B*sin(bta)) # psi - zNext[7] = z[7] + dt*z[8] # psi - zNext[8] = z[8] # psidot - zNext[9] = z[9] # drift_psi - return zNext -end - -function numerical_jac(f,x,args) - y = f(x,args) - jac = zeros(size(y,1),size(x,1)) - eps = 1e-5 - xp = copy(x) - for i = 1:size(x,1) - xp[i] = x[i]+eps/2.0 - yhi = f(xp,args) - xp[i] = x[i]-eps/2.0 - ylo = f(xp,args) - xp[i] = x[i] - jac[:,i] = (yhi-ylo)/eps - end - return jac -end - -function initPlot() # Initialize Plots for export - linewidth = 0.4 - rc("axes", linewidth=linewidth) - rc("lines", linewidth=linewidth, markersize=2) - #rc("font", family="") - rc("axes", titlesize="small", labelsize="small") # can be set in Latex - rc("xtick", labelsize="x-small") - rc("xtick.major", width=linewidth/2) - rc("ytick", labelsize="x-small") - rc("ytick.major", width=linewidth/2) - rc("legend", fontsize="small") - rc("font",family="serif") - rc("font",size=8) - rc("figure",figsize=[4.5,3]) - #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") -end - -function unwrap!(p) - length(p) < 2 && return p - for i = 2:length(p) - d = p[i] - p[i-1] - if abs(d) > pi - p[i] -= floor((d+pi) / (2*pi)) * 2pi - end - end - return p -end - -function rotMatrix(s::Char,deg::Float64) - A = zeros(3,3) - if s=='x' - A = [1 0 0; - 0 cos(deg) sin(deg); - 0 -sin(deg) cos(deg)] - elseif s=='y' - A = [cos(deg) 0 -sin(deg); - 0 1 0; - sin(deg) 0 cos(deg)] - elseif s=='z' - A = [cos(deg) sin(deg) 0 - -sin(deg) cos(deg) 0 - 0 0 1] - else - warn("Wrong angle for rotation matrix") - end - return A -end diff --git a/eval_sim/Kalman simulation/sim_kalman_mix.jl b/eval_sim/Kalman simulation/sim_kalman_mix.jl deleted file mode 100644 index 7149af39..00000000 --- a/eval_sim/Kalman simulation/sim_kalman_mix.jl +++ /dev/null @@ -1,386 +0,0 @@ -# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. -# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. - -using PyPlot -using JLD - -Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) -R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) - -# pos_info[1] = s -# pos_info[2] = eY -# pos_info[3] = ePsi -# pos_info[4] = v -# pos_info[5] = s_start -# pos_info[6] = x -# pos_info[7] = y -# pos_info[8] = v_x -# pos_info[9] = v_y -# pos_info[10] = psi -# pos_info[11] = psiDot -# pos_info[12] = x_raw -# pos_info[13] = y_raw -# pos_info[14] = psi_raw -# pos_info[15] = v_raw -# pos_info[16] = psi_drift - - -function main(code::AbstractString) - global Q, R, R_gps_imu - log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - d_rec = load(log_path_record) - - imu_meas = d_rec["imu_meas"] - gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - cmd_pwm_log = d_rec["cmd_pwm_log"] - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] - - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) - - l_A = 0.125 - l_B = 0.125 - - dt = 1/25 - t = t0:dt:t_end - sz = length(t) - y = zeros(sz,6) - u = zeros(sz,2) - - y_gps_imu = zeros(sz,7) - - P = zeros(7,7) - x_est = zeros(length(t),7) - P_gps_imu = zeros(11,11) - x_est_gps_imu = zeros(length(t),11) - #x_est_gps_imu[1,11] = 0.5 - #x_est_gps_imu[1,10] = -0.6 - - yaw0 = imu_meas.z[1,6] - gps_dist = zeros(length(t)) - - yaw_prev = yaw0 - - #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.01,0.01,0.01]) - Q_gps_imu = diagm([0.01,0.01,1/2*dt^2,1/2*dt^2,0.001,0.001,0.01,0.01,0.00001,0.00001,0.00001]) - R_gps_imu = diagm([0.1,0.1,0.1,0.1,0.1,1.0,1.0]) - - att_raw = zeros(sz,3) - att_normalized = zeros(sz,3) - - acc_raw = zeros(sz,3) - acc_normalized = zeros(sz,3) - acc_inert = zeros(sz,3) - - omega_normalized = zeros(sz,3) - - # Calibrate IMU: - t_start = cmd_log.t[1] - imu_att_cal = mean(imu_meas.z[imu_meas.t.t[i],:][1,:] - y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 - y_yawdot = imu_meas.z[imu_meas.t.>t[i],3][1] - a_x = imu_meas.z[imu_meas.t.>t[i],7][1] - a_y = imu_meas.z[imu_meas.t.>t[i],8][1] - y_vel_est = vel_est.z[vel_est.t.>t[i]][1] - - acc_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],7:9][1,:] - att_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],4:6][1,:] - BI_A = eulerTrans([att_raw[i,1:2] 0]) - acc_normalized[i,:] = BI_A'*acc_raw[i,:]' - omega_normalized[i,:] = BI_A'*imu_meas.z[imu_meas.t.>t[i],1:3][1,:]' - #acc_inert[i,:] = eulerTrans([imu_meas.z[imu_meas.t.>t[i],4:5][1,:] y_yaw])'*acc_raw[i,:]' - y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] - #y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot acc_normalized[i,1:2]] - y[:,3] = unwrap!(y[:,3]) - y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) - #y_gps_imu[i,6:7] = [0 0] - - u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] - - #att_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],4:6][1,:] - #att_normalized[i,:] = Z_IMU_A' * att_raw[i,:]' - #att_normalized[i,:] = angRates2EulerRates(att_raw[i,:],[-imu_meas.z[imu_meas.t.>t[i],4:5][1,1:2] imu_meas.z[imu_meas.t.>t[i],6][1]]) - #att_normalized[i,:] = rotMatrix('y',imu_meas.z[imu_meas.t.>t[i],5][1])*rotMatrix('x',imu_meas.z[imu_meas.t.>t[i],4][1])*att_raw[i,:]' - # Adapt R-value of GPS according to distance to last point: - gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - if gps_dist[i] > 0.2 - R[1,1] = 1+10*gps_dist[i]^2 - R[2,2] = 1+10*gps_dist[i]^2 - R_gps_imu[1,1] = 0.1+100*gps_dist[i]^2 - R_gps_imu[2,2] = 0.1+100*gps_dist[i]^2 - else - R[1,1] = 1 - R[2,2] = 1 - R_gps_imu[1,1] = 0.1 - R_gps_imu[2,2] = 0.1 - end - - - args = (u[i,:],dt,l_A,l_B) - - # Calculate new estimate - (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) - (x_est_gps_imu[i,:], P_gps_imu) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) - end - - figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,pos_info.t-t0,pos_info.z[:,6:7],"-x") - grid("on") - title("Comparison x,y estimate and measurement") - - figure(2) - plot(t-t0,x_est_gps_imu[:,9:11]) - grid("on") - title("Drifts") - legend(["a_x","a_y","psi"]) - - figure(3) - v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z,pos_info.t-t0,pos_info.z[:,8:9],"-x") - grid("on") - legend(["v","v_x_est","v_y_est","v_meas"]) - title("Velocity estimate and measurement") - - figure(4) - plot(t-t0,x_est_gps_imu[:,7],"-*",t-t0,y_gps_imu[:,4]) - grid("on") - title("Comparison yaw") - - figure(5) - plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,8],"-*") - grid("on") - - figure(6) - #plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) - plot(t-t0,acc_normalized[:,1:2],t-t0,x_est_gps_imu[:,5:6]) - grid("on") - # ax1=subplot(211) - # plot(t,y,"-x",t,x_est,"-*") - # grid("on") - # legend(["x","y","psi","v"]) - # subplot(212,sharex=ax1) - # plot(t,u) - # grid("on") - # legend(["a_x","d_f"]) - # figure(1) - # ax=subplot(5,1,1) - # for i=1:4 - # subplot(5,1,i,sharex=ax) - # plot(t,y[:,i],t,x_est[:,i],"-*") - # grid("on") - # end - # subplot(5,1,5,sharex=ax) - # plot(cmd_pwm_log.t,cmd_pwm_log.z) - # grid("on") - - - # figure(2) - # subplot(2,1,1) - # title("Comparison raw GPS data and estimate") - # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["x_m","x_est"]) - # grid("on") - # subplot(2,1,2) - # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["y_m","y_est"]) - # grid("on") - - # figure(3) - # plot(t,gps_dist) - # title("GPS distances") - # grid("on") - # # figure() - # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") - # # grid("on") - # # title("x-y-view") - # figure(4) - # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") - # title("Comparison simulation (-x) onboard-estimate (-*)") - # grid("on") - # legend(["x","y","psi","v","psi_drift"]) - - # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) - # figure(5) - # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) - # title("Comparison yaw") - # grid("on") - - # figure(6) - # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) - # grid("on") - # title("Comparison of velocity measurement and estimate") - # legend(["estimate","measurement"]) - - #figure(7) - #plt[:hist](gps_dist,300) - #grid("on") - nothing -end - -function h(x,args) - C = [eye(6) zeros(6,1)] - C[4,4] = 1 - #C[3,3] = 0 - C[3,5] = 1 - C[5,5] = 0 - C[5,6] = 1 - C[6,6] = 0 - C[6,7] = 1 - return C*x -end -function h_gps_imu(x,args) - y = zeros(7) - y[1] = x[1] # x - y[2] = x[2] # y - y[3] = sqrt(x[3]^2+x[4]^2) # v - y[4] = x[7]+x[11] # psi - y[5] = x[8] # psiDot - y[6] = x[5]+x[9] # a_x - y[7] = x[6]+x[10] # a_y - return y -end - -function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1 -end - -function simModel(z,args) - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - - zNext = copy(z) - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - #zNext[4] = z[4] + dt*(z[6]) # v - #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - zNext[5] = z[5] # psi_drift - zNext[6] = z[6] # a_x - zNext[7] = z[7] # a_y - return zNext -end - -function simModel_gps_imu(z,args) - (u,dt,l_A,l_B) = args - #bta = atan(l_A/(l_A+l_B)*tan(u[2])) - bta = atan2(z[4],z[3]) - zNext = copy(z) - zNext[1] = z[1] + dt*(sqrt(z[3]^2+z[4]^2)*cos(z[7] + bta)) # x - zNext[2] = z[2] + dt*(sqrt(z[3]^2+z[4]^2)*sin(z[7] + bta)) # y - zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x - zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y - zNext[5] = z[5] # a_x - zNext[6] = z[6] # a_y - zNext[7] = z[7] + dt*(sqrt(z[3]^2+z[4]^2)/l_B*sin(bta)) # psi - zNext[8] = z[8] # psidot - zNext[9] = z[9] # drift_a_x - zNext[10] = z[10] # drift_a_y - zNext[11] = z[11] # drift_psi - return zNext -end - -function numerical_jac(f,x,args) - y = f(x,args) - jac = zeros(size(y,1),size(x,1)) - eps = 1e-5 - xp = copy(x) - for i = 1:size(x,1) - xp[i] = x[i]+eps/2.0 - yhi = f(xp,args) - xp[i] = x[i]-eps/2.0 - ylo = f(xp,args) - xp[i] = x[i] - jac[:,i] = (yhi-ylo)/eps - end - return jac -end - -function initPlot() # Initialize Plots for export - linewidth = 0.4 - rc("axes", linewidth=linewidth) - rc("lines", linewidth=linewidth, markersize=2) - #rc("font", family="") - rc("axes", titlesize="small", labelsize="small") # can be set in Latex - rc("xtick", labelsize="x-small") - rc("xtick.major", width=linewidth/2) - rc("ytick", labelsize="x-small") - rc("ytick.major", width=linewidth/2) - rc("legend", fontsize="small") - rc("font",family="serif") - rc("font",size=8) - rc("figure",figsize=[4.5,3]) - #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") -end - -function unwrap!(p) - length(p) < 2 && return p - for i = 2:length(p) - d = p[i] - p[i-1] - if abs(d) > pi - p[i] -= floor((d+pi) / (2*pi)) * 2pi - end - end - return p -end - -function rotMatrix(s::Char,deg::Float64) - A = zeros(3,3) - if s=='x' - A = [1 0 0; - 0 cos(deg) sin(deg); - 0 -sin(deg) cos(deg)] - elseif s=='y' - A = [cos(deg) 0 -sin(deg); - 0 1 0; - sin(deg) 0 cos(deg)] - elseif s=='z' - A = [cos(deg) sin(deg) 0 - -sin(deg) cos(deg) 0 - 0 0 1] - else - warn("Wrong angle for rotation matrix") - end - return A -end - -function angRates2EulerRates(w::Array{Float64},deg::Array{Float64}) - (p,q,r) = w - (phi,theta,psi) = deg - phidot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r - thetadot = cos(phi)*q - sin(phi)*r - psidot = sin(phi)*sec(theta)*q + cos(phi)*sec(theta)*r - return [phidot,thetadot,psidot] -end - -function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body - return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) -end \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl b/eval_sim/Kalman simulation/sim_kalman_mix_2.jl deleted file mode 100644 index 180f0a52..00000000 --- a/eval_sim/Kalman simulation/sim_kalman_mix_2.jl +++ /dev/null @@ -1,412 +0,0 @@ -# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. -# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. - -using PyPlot -using JLD - -Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) -R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) - -# pos_info[1] = s -# pos_info[2] = eY -# pos_info[3] = ePsi -# pos_info[4] = v -# pos_info[5] = s_start -# pos_info[6] = x -# pos_info[7] = y -# pos_info[8] = v_x -# pos_info[9] = v_y -# pos_info[10] = psi -# pos_info[11] = psiDot -# pos_info[12] = x_raw -# pos_info[13] = y_raw -# pos_info[14] = psi_raw -# pos_info[15] = v_raw -# pos_info[16] = psi_drift - - -function main(code::AbstractString) - global Q, R, R_gps_imu - #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - log_path_record = "$(homedir())/simulations/output-record-$(code).jld" - d_rec = load(log_path_record) - - imu_meas = d_rec["imu_meas"] - gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - cmd_pwm_log = d_rec["cmd_pwm_log"] - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] - - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_pwm_log.t[1])+0.0 - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end])-0.0 - - l_A = 0.125 - l_B = 0.125 - - dt = 1/50 - t = t0:dt:t_end - sz = length(t) - y = zeros(sz,6) - u = zeros(sz,2) - - y_gps_imu = zeros(sz,5) - - x_est = zeros(length(t),6) - P_gps_imu = zeros(6,6) - x_est_gps_imu = zeros(length(t),8) - #x_est_gps_imu[1,11] = 0.5 - #x_est_gps_imu[1,10] = -0.6 - - yaw0 = imu_meas.z[1,6] - gps_dist = zeros(length(t)) - - yaw_prev = yaw0 - - Q_gps_imu = diagm([0.1,0.1,1.0,0.1,0.1,0.01]) - R_gps_imu = diagm([1.0,1.0,10000.0,0.1,5.0]) - - gps_gate = zeros(length(t)) - - gps_prev = gps_meas.z[1,:] - vel_est_prev = vel_est.z[1,1] - imu_prev = imu_meas.z[1,6] - # measured: IMU_a - # calibrated: Z_a - # need matrix Z_IMU_A - for i=2:length(t) - # Collect measurements and inputs for this iteration - y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] - y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 - y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] - a_x = imu_meas.z[t[i].>imu_meas.t,7][end] - a_y = imu_meas.z[t[i].>imu_meas.t,8][end] - y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] - - y_gps_imu[i,:] = [y_gps y_yaw y_vel_est y_yawdot] - y_gps_imu[:,3] = unwrap!(y_gps_imu[:,3]) - - u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] - u[i,2] = cmd_log.z[t[i].>cmd_log.t-0.0,2][end] - - gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - #gps_gate[i] = - # if gps_dist[i] > 1.0 - # R[1,1] = 1+10*gps_dist[i]^2 - # R[2,2] = 1+10*gps_dist[i]^2 - # R_gps_imu[1,1] = 1.0+100*gps_dist[i]^2 - # R_gps_imu[2,2] = 1.0+100*gps_dist[i]^2 - # else - # R[1,1] = 1 - # R[2,2] = 1 - # R_gps_imu[1,1] = 0.1 - # R_gps_imu[2,2] = 0.1 - # end - if gps_prev == y_gps - R_gps_imu[1:2,1:2] = 100.0*eye(2) - else - R_gps_imu[1:2,1:2] = 10.0*eye(2) - end - if vel_est_prev == y_vel_est - R_gps_imu[4,4] = 10.0 - else - R_gps_imu[4,4] = 0.1 - end - if imu_prev == y_yaw - R_gps_imu[3,3] = 100000.0 - R_gps_imu[5,5] = 10.0 - else - R_gps_imu[3,3] = 0.1 - R_gps_imu[5,5] = 5.0 - end - gps_prev = y_gps - vel_est_prev = y_vel_est - imu_prev = y_yaw - args = (u[i,:],dt,l_A,l_B) - - # Calculate new estimate - (x_est_gps_imu[i,1:6], P_gps_imu, gps_gate[i]) = ekf_gate(simModel_gps_imu,x_est_gps_imu[i-1,1:6]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) - beta = atan(l_A*tan(u[i,2])/(l_B+l_A)) - x_est_gps_imu[i,7] = cos(beta)*x_est_gps_imu[i,4] # v_x - x_est_gps_imu[i,8] = l_B*x_est_gps_imu[i,5] # v_y - end - - figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-x",pos_info.t-t0,pos_info.z[:,6:7],"--") - grid("on") - title("Comparison x,y estimate and measurement") - legend(["x_est","y_est","x_meas","y_meas","x_onboard","y_onboard"]) - - figure(2) - plot(t-t0,x_est_gps_imu[:,6]) - grid("on") - title("Drifts") - legend(["psi"]) - - figure(3) - v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z,pos_info.t-t0,pos_info.z[:,8:9],"-x") - grid("on") - legend(["v","v_x_est","v_y_est","v_meas","v_x_onboard","v_y_onboard"]) - title("Velocity estimate and measurement") - - figure(4) - plot(t-t0,y_gps_imu[:,3],t-t0,x_est_gps_imu[:,3],"-*",pos_info.t-t0,pos_info.z[:,10]) - grid("on") - title("Comparison yaw") - legend(["psi_meas","psi_est","psi_onboard"]) - - # figure(5) - # plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,5]) - # title("w_z") - # legend(["w_z_meas","w_z_est"]) - # grid("on") - - # figure(6) - # plot(t-t0,x_est_gps_imu[:,7:8]) - # grid("on") - # legend(["v_x_est","v_y_est"]) - - # figure(7) - # plot(t-t0,gps_gate,t-t0,gps_dist,gps_meas.t-t0,gps_meas.z) - # grid("on") - # legend(["gate","dist"]) - - figure(8) - plot(x_est_gps_imu[:,1],x_est_gps_imu[:,2]) - for i=1:5:size(x_est_gps_imu,1) - ar = [x_est_gps_imu[i,1] x_est_gps_imu[i,2]; - x_est_gps_imu[i,1]+0.2*cos(x_est_gps_imu[i,3]) x_est_gps_imu[i,2]+0.2*sin(x_est_gps_imu[i,3])] - plot(ar[:,1],ar[:,2]) - end - axis("equal") - grid("on") - - # # CORRELATIONS: - # figure(8) - # plot(94.14+2.7678*u[:,1],y[:,4],"*") - # grid("on") - # title("Comparison motor input and velocity") - # xlabel("PWM signal") - # ylabel("v [m/s]") - - # delta = atan(x_est_gps_imu[:,8]*0.25./x_est_gps_imu[:,3]) - # #delta2 = atan(0.25/sqrt(y_gps_imu[:,3].^2./y_gps_imu[:,5].^2-0.125^2)) - # figure(9) - # plot(u[:,2],delta,"*") - # grid("on") - - # ax1=subplot(211) - # plot(t,y,"-x",t,x_est,"-*") - # grid("on") - # legend(["x","y","psi","v"]) - # subplot(212,sharex=ax1) - # plot(t,u) - # grid("on") - # legend(["a_x","d_f"]) - # figure(1) - # ax=subplot(5,1,1) - # for i=1:4 - # subplot(5,1,i,sharex=ax) - # plot(t,y[:,i],t,x_est[:,i],"-*") - # grid("on") - # end - # subplot(5,1,5,sharex=ax) - # plot(cmd_pwm_log.t,cmd_pwm_log.z) - # grid("on") - - - # figure(2) - # subplot(2,1,1) - # title("Comparison raw GPS data and estimate") - # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["x_m","x_est"]) - # grid("on") - # subplot(2,1,2) - # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["y_m","y_est"]) - # grid("on") - - # figure(3) - # plot(t,gps_dist) - # title("GPS distances") - # grid("on") - # # figure() - # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") - # # grid("on") - # # title("x-y-view") - # figure(4) - # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") - # title("Comparison simulation (-x) onboard-estimate (-*)") - # grid("on") - # legend(["x","y","psi","v","psi_drift"]) - - # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) - # figure(5) - # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) - # title("Comparison yaw") - # grid("on") - - # figure(6) - # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) - # grid("on") - # title("Comparison of velocity measurement and estimate") - # legend(["estimate","measurement"]) - - #figure(7) - #plt[:hist](gps_dist,300) - #grid("on") - nothing -end - -function h_gps_imu(x,args) - y = zeros(5) - y[1] = x[1] # x - y[2] = x[2] # y - y[3] = x[3]+x[6] # psi - y[4] = x[4] # v - y[5] = x[5] # psiDot - return y -end - -function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1 -end - -function ekf_gate(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - S = H*P12 - gps_err = ((y_kp1[1:2]-my_kp1[1:2])'*inv(S[1:2,1:2])*(y_kp1[1:2]-my_kp1[1:2]))[1] - # if gps_err > 100.0 - # R[1:2,1:2] = eye(2)*100 - # else - # R[1:2,1:2] = eye(2)*0.1 - # end - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1, gps_err -end - -function simModel_gps_imu(z,args) - (u,dt,l_A,l_B) = args - #bta = atan(l_A/(l_A+l_B)*tan(u[2])) - bta = atan2(l_A*tan(u[2]),l_A+l_B) - zNext = copy(z) - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(u[1]-1.0*z[4]) # v - zNext[5] = z[5] # psiDot - zNext[6] = z[6] # drift_psi - return zNext -end - -function numerical_jac(f,x,args) - y = f(x,args) - jac = zeros(size(y,1),size(x,1)) - eps = 1e-5 - xp = copy(x) - for i = 1:size(x,1) - xp[i] = x[i]+eps/2.0 - yhi = f(xp,args) - xp[i] = x[i]-eps/2.0 - ylo = f(xp,args) - xp[i] = x[i] - jac[:,i] = (yhi-ylo)/eps - end - return jac -end - -function initPlot() # Initialize Plots for export - linewidth = 0.4 - rc("axes", linewidth=linewidth) - rc("lines", linewidth=linewidth, markersize=2) - #rc("font", family="") - rc("axes", titlesize="small", labelsize="small") # can be set in Latex - rc("xtick", labelsize="x-small") - rc("xtick.major", width=linewidth/2) - rc("ytick", labelsize="x-small") - rc("ytick.major", width=linewidth/2) - rc("legend", fontsize="small") - rc("font",family="serif") - rc("font",size=8) - rc("figure",figsize=[4.5,3]) - #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") -end - -function unwrap!(p) - length(p) < 2 && return p - for i = 2:length(p) - d = p[i] - p[i-1] - if abs(d) > pi - p[i] -= floor((d+pi) / (2*pi)) * 2pi - end - end - return p -end - -function rotMatrix(s::Char,deg::Float64) - A = zeros(3,3) - if s=='x' - A = [1 0 0; - 0 cos(deg) sin(deg); - 0 -sin(deg) cos(deg)] - elseif s=='y' - A = [cos(deg) 0 -sin(deg); - 0 1 0; - sin(deg) 0 cos(deg)] - elseif s=='z' - A = [cos(deg) sin(deg) 0 - -sin(deg) cos(deg) 0 - 0 0 1] - else - warn("Wrong angle for rotation matrix") - end - return A -end - -function angRates2EulerRates(w::Array{Float64},deg::Array{Float64}) - (p,q,r) = w - (phi,theta,psi) = deg - phidot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r - thetadot = cos(phi)*q - sin(phi)*r - psidot = sin(phi)*sec(theta)*q + cos(phi)*sec(theta)*r - return [phidot,thetadot,psidot] -end - -function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body - return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) -end - -function getIndex(t_raw::Array{Float64},t::Float64) - i = 1 - while i<=length(t_raw) - if t > t_raw[i] - i += 1 - else - break - end - end - return i -end \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_mix_nodrift.jl b/eval_sim/Kalman simulation/sim_kalman_mix_nodrift.jl deleted file mode 100644 index cd153903..00000000 --- a/eval_sim/Kalman simulation/sim_kalman_mix_nodrift.jl +++ /dev/null @@ -1,443 +0,0 @@ -# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. -# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. - -using PyPlot -using JLD - -Q = diagm([0.1,0.1,0.1,0.1,0.1,0.1,0.1]) -R = diagm([0.1,0.1,0.1,1.0,10.0,10.0]) - -# pos_info[1] = s -# pos_info[2] = eY -# pos_info[3] = ePsi -# pos_info[4] = v -# pos_info[5] = s_start -# pos_info[6] = x -# pos_info[7] = y -# pos_info[8] = v_x -# pos_info[9] = v_y -# pos_info[10] = psi -# pos_info[11] = psiDot -# pos_info[12] = x_raw -# pos_info[13] = y_raw -# pos_info[14] = psi_raw -# pos_info[15] = v_raw -# pos_info[16] = psi_drift - - -function main(code::AbstractString) - global Q, R, R_gps_imu - log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" - d_rec = load(log_path_record) - - imu_meas = d_rec["imu_meas"] - gps_meas = d_rec["gps_meas"] - cmd_log = d_rec["cmd_log"] - cmd_pwm_log = d_rec["cmd_pwm_log"] - vel_est = d_rec["vel_est"] - pos_info = d_rec["pos_info"] - - t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) - t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) - - l_A = 0.125 - l_B = 0.125 - - dt = 1/50 - t = t0:dt:t_end - sz = length(t) - y = zeros(sz,6) - u = zeros(sz,2) - - y_gps_imu = zeros(sz,7) - - P = zeros(7,7) - x_est = zeros(length(t),7) - P_gps_imu = zeros(9,9) - x_est_gps_imu = zeros(length(t),9) - #x_est_gps_imu[1,11] = 0.5 - #x_est_gps_imu[1,10] = -0.6 - - yaw0 = imu_meas.z[1,6] - gps_dist = zeros(length(t)) - - yaw_prev = yaw0 - - #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.01,0.01,0.01]) - #Q_gps_imu = diagm([0.001,0.001,0.001,0.001,0.001,0.001,0.01,0.1,0.00001]) - #R_gps_imu = diagm([0.1,0.1,0.1,0.001,0.01,0.01,0.01]) - - Q_gps_imu = diagm([0.01,0.01,0.1,0.1,0.1,0.1,0.01,0.1,0.0000001]) - R_gps_imu = diagm([0.1,0.1,10.0,0.01,1.0,10.0,10.0]) - - att_raw = zeros(sz,3) - att_normalized = zeros(sz,3) - - acc_raw = zeros(sz,3) - acc_normalized = zeros(sz,3) - acc_inert = zeros(sz,3) - - omega_normalized = zeros(sz,3) - - gps_gate = zeros(length(t)) - # Calibrate IMU: - t_start = cmd_log.t[1] - imu_att_cal = mean(imu_meas.z[imu_meas.t.gps_meas.t,:][end,:] - y_yaw = imu_meas.z[t[i].>imu_meas.t,6][end]-yaw0 - y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] - a_x = imu_meas.z[t[i].>imu_meas.t,7][end] - a_y = imu_meas.z[t[i].>imu_meas.t,8][end] - y_vel_est = vel_est.z[t[i].>vel_est.t][end] - - acc_raw[i,:] = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] - att_raw[i,:] = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] - #BI_A = eulerTrans([att_raw[i,1:2] 0]) - BI_A = eye(3) - acc_normalized[i,:] = BI_A'*acc_raw[i,:]' - omega_normalized[i,:] = BI_A'*imu_meas.z[t[i].>imu_meas.t,1:3][end,:]' - #acc_inert[i,:] = eulerTrans([imu_meas.z[imu_meas.t.>t[i],4:5][1,:] y_yaw])'*acc_raw[i,:]' - y[i,:] = [y_gps y_yaw y_vel_est a_x a_y] - #y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y] - y_gps_imu[i,:] = [y_gps y_vel_est y_yaw omega_normalized[i,3] acc_normalized[i,1:2]] - y[:,3] = unwrap!(y[:,3]) - y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) - #y_gps_imu[i,6:7] = [0 0] - - u[i,:] = cmd_log.z[t[i].>cmd_log.t,:][end,:] - - #att_raw[i,:] = imu_meas.z[imu_meas.t.>t[i],4:6][1,:] - #att_normalized[i,:] = Z_IMU_A' * att_raw[i,:]' - #att_normalized[i,:] = angRates2EulerRates(att_raw[i,:],[-imu_meas.z[imu_meas.t.>t[i],4:5][1,1:2] imu_meas.z[imu_meas.t.>t[i],6][1]]) - #att_normalized[i,:] = rotMatrix('y',imu_meas.z[imu_meas.t.>t[i],5][1])*rotMatrix('x',imu_meas.z[imu_meas.t.>t[i],4][1])*att_raw[i,:]' - # Adapt R-value of GPS according to distance to last point: - gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) - #gps_gate[i] = - # if gps_dist[i] > 1.0 - # R[1,1] = 1+10*gps_dist[i]^2 - # R[2,2] = 1+10*gps_dist[i]^2 - # R_gps_imu[1,1] = 1.0+100*gps_dist[i]^2 - # R_gps_imu[2,2] = 1.0+100*gps_dist[i]^2 - # else - # R[1,1] = 1 - # R[2,2] = 1 - # R_gps_imu[1,1] = 0.1 - # R_gps_imu[2,2] = 0.1 - # end - if gps_prev == y_gps - R_gps_imu[1:2,1:2] = 10*eye(2) - else - R_gps_imu[1:2,1:2] = 0.1*eye(2) - end - gps_prev = y_gps - args = (u[i,:],dt,l_A,l_B) - - # Calculate new estimate - (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) - (x_est_gps_imu[i,:], P_gps_imu, gps_gate[i]) = ekf_gate(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) - end - - figure(1) - plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,pos_info.t-t0,pos_info.z[:,6:7],"-x") - grid("on") - title("Comparison x,y estimate and measurement") - legend(["x_est","y_est","x_meas","y_meas","x_onboard","y_onboard"]) - - figure(2) - plot(t-t0,x_est_gps_imu[:,9]) - grid("on") - title("Drifts") - legend(["psi"]) - - figure(3) - v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) - plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,3:4],"--",vel_est.t-t0,vel_est.z,pos_info.t-t0,pos_info.z[:,8:9],"-x") - grid("on") - legend(["v","v_x_est","v_y_est","v_meas","v_x_onboard","v_y_onboard"]) - title("Velocity estimate and measurement") - - figure(4) - plot(t-t0,x_est_gps_imu[:,7],"-*",t-t0,y_gps_imu[:,4]) - grid("on") - title("Comparison yaw") - legend(["psi_est","psi_meas"]) - - figure(5) - plot(imu_meas.t-t0,imu_meas.z[:,3],t-t0,x_est_gps_imu[:,8]) - title("w_z") - legend(["w_z_meas","w_z_est"]) - grid("on") - - figure(6) - #plot(imu_meas.t-t0,imu_meas.z[:,7:8],t-t0,x_est_gps_imu[:,5:6]) - plot(t-t0,acc_normalized[:,1:2],t-t0,x_est_gps_imu[:,5:6]) - title("Accelerations") - legend(["a_x_meas","a_y_meas","a_x_est","a_y_est"]) - grid("on") - - figure(7) - plot(t-t0,gps_gate,t-t0,gps_dist,gps_meas.t-t0,gps_meas.z) - grid("on") - legend(["gate","dist"]) - - # CORRELATIONS: - figure(8) - plot(94.14+2.7678*u[:,1],y[:,4],"*") - grid("on") - title("Comparison motor input and velocity") - xlabel("PWM signal") - ylabel("v [m/s]") - - delta = atan(x_est_gps_imu[:,8]*0.25./x_est_gps_imu[:,3]) - #delta2 = atan(0.25/sqrt(y_gps_imu[:,3].^2./y_gps_imu[:,5].^2-0.125^2)) - figure(9) - plot(u[:,2],delta,"*") - grid("on") - - # ax1=subplot(211) - # plot(t,y,"-x",t,x_est,"-*") - # grid("on") - # legend(["x","y","psi","v"]) - # subplot(212,sharex=ax1) - # plot(t,u) - # grid("on") - # legend(["a_x","d_f"]) - # figure(1) - # ax=subplot(5,1,1) - # for i=1:4 - # subplot(5,1,i,sharex=ax) - # plot(t,y[:,i],t,x_est[:,i],"-*") - # grid("on") - # end - # subplot(5,1,5,sharex=ax) - # plot(cmd_pwm_log.t,cmd_pwm_log.z) - # grid("on") - - - # figure(2) - # subplot(2,1,1) - # title("Comparison raw GPS data and estimate") - # plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["x_m","x_est"]) - # grid("on") - # subplot(2,1,2) - # plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") - # xlabel("t [s]") - # ylabel("Position [m]") - # legend(["y_m","y_est"]) - # grid("on") - - # figure(3) - # plot(t,gps_dist) - # title("GPS distances") - # grid("on") - # # figure() - # # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") - # # grid("on") - # # title("x-y-view") - # figure(4) - # plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") - # title("Comparison simulation (-x) onboard-estimate (-*)") - # grid("on") - # legend(["x","y","psi","v","psi_drift"]) - - # unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) - # figure(5) - # plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) - # title("Comparison yaw") - # grid("on") - - # figure(6) - # plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) - # grid("on") - # title("Comparison of velocity measurement and estimate") - # legend(["estimate","measurement"]) - - #figure(7) - #plt[:hist](gps_dist,300) - #grid("on") - nothing -end - -function h(x,args) - C = [eye(6) zeros(6,1)] - C[4,4] = 1 - #C[3,3] = 0 - C[3,5] = 1 - C[5,5] = 0 - C[5,6] = 1 - C[6,6] = 0 - C[6,7] = 1 - return C*x -end -function h_gps_imu(x,args) - y = zeros(7) - y[1] = x[1] # x - y[2] = x[2] # y - y[3] = sqrt(x[3]^2+x[4]^2) # v - y[4] = x[7]+x[9] # psi - y[5] = x[8] # psiDot - y[6] = x[5] # a_x - y[7] = x[6] # a_y - return y -end - -function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1 -end - -function ekf_gate(f, mx_k, P_k, h, y_kp1, Q, R, args) - xDim = size(mx_k,1) - mx_kp1 = f(mx_k,args) - A = numerical_jac(f,mx_k,args) - P_kp1 = A*P_k*A' + Q - my_kp1 = h(mx_kp1,args) - H = numerical_jac(h,mx_kp1,args) - P12 = P_kp1*H' - S = H*P12 - gps_err = ((y_kp1[1:2]-my_kp1[1:2])'*inv(S[1:2,1:2])*(y_kp1[1:2]-my_kp1[1:2]))[1] - # if gps_err > 100.0 - # R[1:2,1:2] = eye(2)*100 - # else - # R[1:2,1:2] = eye(2)*0.1 - # end - K = P12*inv(H*P12+R) - mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) - P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' - return mx_kp1, P_kp1, gps_err -end - -function simModel(z,args) - # kinematic bicycle model - # u[1] = acceleration - # u[2] = steering angle - (u,dt,l_A,l_B) = args - bta = atan(l_A/(l_A+l_B)*tan(u[2])) - - zNext = copy(z) - zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x - zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y - zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi - zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - #zNext[4] = z[4] + dt*(z[6]) # v - #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v - zNext[5] = z[5] # psi_drift - zNext[6] = z[6] # a_x - zNext[7] = z[7] # a_y - return zNext -end - -function simModel_gps_imu(z,args) - (u,dt,l_A,l_B) = args - #bta = atan(l_A/(l_A+l_B)*tan(u[2])) - bta = atan2(z[4],z[3]) - zNext = copy(z) - zNext[1] = z[1] + dt*(sqrt(z[3]^2+z[4]^2)*cos(z[7] + bta)) # x - zNext[2] = z[2] + dt*(sqrt(z[3]^2+z[4]^2)*sin(z[7] + bta)) # y - zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x - zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y - zNext[5] = z[5] # a_x - zNext[6] = z[6] # a_y - zNext[7] = z[7] + dt*(sqrt(z[3]^2+z[4]^2)/l_B*sin(bta)) # psi - zNext[8] = z[8] # psidot - zNext[9] = z[9] # drift_psi - return zNext -end - -function numerical_jac(f,x,args) - y = f(x,args) - jac = zeros(size(y,1),size(x,1)) - eps = 1e-5 - xp = copy(x) - for i = 1:size(x,1) - xp[i] = x[i]+eps/2.0 - yhi = f(xp,args) - xp[i] = x[i]-eps/2.0 - ylo = f(xp,args) - xp[i] = x[i] - jac[:,i] = (yhi-ylo)/eps - end - return jac -end - -function initPlot() # Initialize Plots for export - linewidth = 0.4 - rc("axes", linewidth=linewidth) - rc("lines", linewidth=linewidth, markersize=2) - #rc("font", family="") - rc("axes", titlesize="small", labelsize="small") # can be set in Latex - rc("xtick", labelsize="x-small") - rc("xtick.major", width=linewidth/2) - rc("ytick", labelsize="x-small") - rc("ytick.major", width=linewidth/2) - rc("legend", fontsize="small") - rc("font",family="serif") - rc("font",size=8) - rc("figure",figsize=[4.5,3]) - #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") -end - -function unwrap!(p) - length(p) < 2 && return p - for i = 2:length(p) - d = p[i] - p[i-1] - if abs(d) > pi - p[i] -= floor((d+pi) / (2*pi)) * 2pi - end - end - return p -end - -function rotMatrix(s::Char,deg::Float64) - A = zeros(3,3) - if s=='x' - A = [1 0 0; - 0 cos(deg) sin(deg); - 0 -sin(deg) cos(deg)] - elseif s=='y' - A = [cos(deg) 0 -sin(deg); - 0 1 0; - sin(deg) 0 cos(deg)] - elseif s=='z' - A = [cos(deg) sin(deg) 0 - -sin(deg) cos(deg) 0 - 0 0 1] - else - warn("Wrong angle for rotation matrix") - end - return A -end - -function angRates2EulerRates(w::Array{Float64},deg::Array{Float64}) - (p,q,r) = w - (phi,theta,psi) = deg - phidot = p + sin(phi)*tan(theta)*q + cos(phi)*tan(theta)*r - thetadot = cos(phi)*q - sin(phi)*r - psidot = sin(phi)*sec(theta)*q + cos(phi)*sec(theta)*r - return [phidot,thetadot,psidot] -end - -function eulerTrans(deg::Array{Float64}) # Transformation matrix inertial -> body - return rotMatrix('x',deg[1])*rotMatrix('y',deg[2])*rotMatrix('z',deg[3]) -end \ No newline at end of file diff --git a/eval_sim/README.md b/eval_sim/README.md new file mode 100644 index 00000000..e72bb0e5 --- /dev/null +++ b/eval_sim/README.md @@ -0,0 +1,78 @@ +Overview over evaluation functions +================================== +All evaluation functions use a code (String) which specifies an experiment. It is a 4-character combination which is created by the ROS master. All files that are saved during one experiment use this code. +Files that are saved: "output-record-(code).jld" from the recorder, "output-LMPC-(code).jld" from the LMPC node, and, if it is a simulation, "output-SIM-(code).jld" (contains real state information). +!! at the beginning denots most used functions. + +eval_data.jl +============ +Main file for all kinds of post-experiment evaluation + +eval_sim(code) +-------------- +Used for general evaluation of *simulations*. It uses 'real' simulation and 'measured' data and compares these. This can be helpful to develop a good state estimator. + +!! eval_run(code) +----------------- +Most general evaluation of experiments. Similar to eval_sim, but does not use simulation data. + +!! eval_LMPC(code) +------------------ +More detailed evaluation of specific LMPC data (MPC cost, sysID coefficients, curvature approximations, ...) + +plot_friction_circle(code, lap) +------------------------------- +Plots a_y over a_x. + +plot_v_ey_over_s(code, laps) +---------------------------- +Plots v and e_y over s for selected laps. Also plots lap times for all laps. + +plot_v_over_xy(code, lap) +------------------------- +Plots velocity in different colors on xy. + +eval_open_loop(code) +-------------------- +Similar to eval_run but for open-loop experiments (which are saved in a different folder). + +eval_predictions_kin(code) +------------------------- +Plots predictions for kinematic model (in path following mode) + +eval_predictions(code) +--------------------- +Plots predictions for dynamic model (in LMPC mode) + +eval_sysID(code) +--------------- +Plots sysID coefficients + +eval_oldTraj(code, lap) +----------------- +Plots data of one selected oldTrajectory (-> data of one specific lap +overlapping data before and after finish line) + +eval_LMPC_coeff(code, step) +-------------------- +Plots terminal constraints (both polynom and prediction) for one specific step in the LMPC process. + +!! anim_LMPC_coeff(code) +-------------------- +Animates predictions and terminal constraints (starting from first LMPC step) + +checkTimes(code) +---------------- +Check solving times and status (optimal/infeasible/user limit). + +Helper functions: +---------------- +create_track(half_track_width) -> used to create the track for plotting +initPlot() -> used to initialize plots for proper export and printing + +sim_kalman_imu.jl +================= +This file simulates a Kalman filter using real sensor data that was recorded during an experiment. It can be used to find the optimal structure and tuning values for the estimator. + +Parameter estimation folder +=========================== +Contains various files and functions to estimate BARC specific input mapping parameters (e.g. plot steering delay, least-square approximation for acceleration and steering, ...) \ No newline at end of file diff --git a/eval_sim/create_track_bezier2.py b/eval_sim/create_track_bezier2.py deleted file mode 100644 index 61dc43a9..00000000 --- a/eval_sim/create_track_bezier2.py +++ /dev/null @@ -1,36 +0,0 @@ -from numpy import * -import matplotlib.pyplot as plt - -x = array([0]) # starting point -y = array([0]) -ds = 0.06 -theta = 0 -d_theta = 0 - -N = 40 - -def create_bezier(p0,p1,p2,p3,dt): - t = arange(0,1+dt,dt)[:,None] - p = (1-t)**3*p0 + 3*(1-t**2)*t*p1+3*(1-t)*t**2*p2+t**3*p3 - return p - -p0 = array([[0,0], - [0,-1], - [0,-2]]) -p1 = array([[1,0], - [-1,-1]]) - -p2 = 2*p0[1,:] - p1[1,:] -p3 = p0[1,:] -for i in range(1,size(p0,0)-2): - p3 = vstack((p3,p0[i,:])) - p2 = vstack((p2,2*p0[i+1]-p1[i+1])) - -p = p0[0,:] -for i in range(0,size(p0,0)-1): - p = vstack((p,create_bezier(p0[i,:],p1[i,:],p2[i,:],p3[i,:].0.01))) - -plt.plot(p[:,0],p[:,1],'-',p0[:,0],p0[:,1],'*') -plt.grid() -plt.axis('equal') -plt.show() diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 95242edb..c7f91a33 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -266,11 +266,6 @@ function plot_v_ey_over_s(code::AbstractString,laps::Array{Int64}) ylim([0,25]) end -function plots_for_presentation() - code = "dc15" - laps = [2,4,6,17,18] -end - function plot_v_over_xy(code::AbstractString,lap::Int64) log_path_record = "$(homedir())/simulations/output-record-$(code).jld" log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" @@ -352,8 +347,6 @@ function eval_open_loop(code::AbstractString) grid("on") end - - # THIS FUNCTION EVALUATES MPC-SPECIFIC DATA # ***************************************** function eval_LMPC(code::AbstractString) @@ -899,66 +892,6 @@ function anim_LMPC_coeff(code::AbstractString) end end - -function anim_LMPC(k1,k2) - d = load(log_path_LMPC) - oldTraj = d["oldTraj"] - sol_z = d["sol_z"] - sol_u = d["sol_u"] - coeffCost = d["coeffCost"] - coeffConst = d["coeffConst"] - s_start = d["s_start"] - state = d["state"] - - N = size(sol_z,1)-1 - - for k=k1:k2 - s = sol_z[1:10,1,k] - subplot(211) - plot(s,sol_u[:,1,k],"-o") - ylim([-1,2]) - xlim([0,2]) - grid() - subplot(212) - plot(s,sol_u[:,2,k],"-o") - ylim([-0.5,0.5]) - xlim([0,2]) - grid() - sleep(0.1) - end - - figure() - hold(0) - for k=k1:k2 - s_state = s_start[k:k+N] + state[k:k+N,1] - s_start[k] - s = sol_z[:,1,k] - ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] - subplot(311) - plot(s,sol_z[:,2,k],"-o",s_state,state[k:k+N,2],"-*",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) - grid() - title("Position = $(s_start[k] + s[1]), k = $k") - xlabel("s") - xlim([0,3]) - ylim([-0.2,0.2]) - ylabel("e_Y") - subplot(312) - plot(s,sol_z[:,3,k],"-o",s_state,state[k:k+N,3],"-*",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) - grid() - xlabel("s") - ylabel("e_Psi") - xlim([0,3]) - ylim([-0.2,0.2]) - subplot(313) - plot(s,sol_z[:,4,k],"-o",s_state,state[k:k+N,4],"-*",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) - grid() - xlabel("s") - ylabel("v") - xlim([0,3]) - ylim([0,2]) - sleep(0.1) - end -end - function visualize_tdiff(code::AbstractString) log_path_record = "$(homedir())/simulations/output-record-$(code).jld" d_rec = load(log_path_record) @@ -1050,6 +983,7 @@ function anim_constraints(code::AbstractString) anim = animation.FuncAnimation(fig, animate, frames=100, interval=50) anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); end + # ***************************************************************** # ****** HELPER FUNCTIONS ***************************************** # ***************************************************************** From ba0e33e56f21d3a6869584e635a835f13bf89a48 Mon Sep 17 00:00:00 2001 From: Ugo Date: Thu, 15 Dec 2016 10:57:49 -0800 Subject: [PATCH 109/183] Changing reference speed in path following as the motor performance has deteriorated and fixing memory allocation variables --- env_loader_pc.sh | 4 ++-- workspace/src/barc/src/Localization_helpers.py | 2 +- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 031bf10e..a9d68f9d 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -1,7 +1,7 @@ #!/bin/bash source /opt/ros/kinetic/setup.bash -source /home/max/barc/workspace/devel/setup.bash -export PATH=$PATH:/home/max/Downloads/julia-2e358ce975/bin +source /home/ugo/GitHub/barc/workspace/devel/setup.bash +export PATH=$PATH:/home/ugo/julia/bin/julia export ROS_IP=192.168.100.72 export ROS_MASTER_URI=http://192.168.100.72:11311 exec "$@" \ No newline at end of file diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index acdee553..c1666956 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -47,7 +47,7 @@ class Localization(object): OrderThetaCurv = 8 # order of theta interpolation closed = True # open or closed trajectory? - coeffCurvature = zeros(4) + coeffCurvature = zeros(OrderThetaCurv + 1) s = 0 # distance from s_start to current closest node (idx_min) s_start = 0 # distance along path from first node to start node (which is N_nodes_poly_back behind current closest node) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index a6b4a379..6c11a7d5 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,7 +47,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,1.0,10.0,0.0] # Q (only for path following mode) - mpcParams.vPathFollowing = 0.8 # reference speed for first lap of path following + mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 1.0*[1,1,1,1,1,0] # cost matrix for derivative cost of states @@ -61,7 +61,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs - mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following + mpcParams_pF.vPathFollowing = 1.0 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay From bfb4c45ac48a4f54dccc2d7c4487c5fa2397d315 Mon Sep 17 00:00:00 2001 From: Ugo Date: Sun, 15 Jan 2017 19:44:45 -0800 Subject: [PATCH 110/183] Settings used for video (1/15/17) --- eval_sim/eval_data.jl | 8 ++++---- workspace/src/barc/src/LMPC_node.jl | 6 +++--- workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl | 8 ++++---- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 6 +++--- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index c7f91a33..d372c36c 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -420,8 +420,8 @@ function eval_LMPC(code::AbstractString) c = zeros(size(curv,1),1) for i=1:size(curv,1) s = state[i,6] - #c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] - c[i] = ([s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] #c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] end plot(state[:,6],c,"-o") @@ -432,8 +432,8 @@ function eval_LMPC(code::AbstractString) s = sol_z[:,6,i] end c = zeros(size(curv,1),1) - #c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' - c = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' #c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]' plot(s,c,"-*") end diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 67cf3a3d..082fb976 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -47,7 +47,7 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po oldTraj.count[lapStatus.currentLap] += 1 # if necessary: append to end of previous lap - if lapStatus.currentLap > 1 && z_est[6] < 16.0 + if lapStatus.currentLap > 1 && z_est[6] < 18.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap-1],6,lapStatus.currentLap-1] += posInfo.s_target oldTraj.oldInput[oldTraj.count[lapStatus.currentLap-1],:,lapStatus.currentLap-1] = [msg.u_a,msg.u_df] @@ -57,7 +57,7 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po end #if necessary: append to beginning of next lap - if z_est[6] > posInfo.s_target - 16.0 + if z_est[6] > posInfo.s_target - 18.0 oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = z_est oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap+1],6,lapStatus.currentLap+1] -= posInfo.s_target oldTraj.oldInput[oldTraj.count[lapStatus.currentLap+1],:,lapStatus.currentLap+1] = [msg.u_a,msg.u_df] @@ -72,7 +72,7 @@ end function main() println("Starting LMPC node.") - buffersize = 3000 # size of oldTraj buffers + buffersize = 7000 # size of oldTraj buffers # Define and initialize variables # --------------------------------------------------------------- diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 4e4438fb..0b70c3be 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -57,7 +57,7 @@ type MpcModel z_Ref = cat(2,v_ref*ones(N+1,1),zeros(N+1,5)) # Reference trajectory: path following -> stay on line and keep constant velocity u_Ref = zeros(N,2) - mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.08))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) @variable( mdl, z_Ol[1:(N+1),1:7]) @variable( mdl, u_Ol[1:N,1:2]) @@ -66,9 +66,9 @@ type MpcModel @variable( mdl, eps[1:N+1] >= 0) # eps for soft lane constraints z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states - z_ub_6s = ones(mpcParams.N+1,1)*[2.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering - u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds + z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(mpcParams.N,1) * [-20.0 -0.3] # lower bounds on steering + u_ub_6s = ones(mpcParams.N,1) * [30.0 0.3] # upper bounds for i=1:2 for j=1:N diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 6c11a7d5..921e8492 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -51,8 +51,8 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 1.0*[1,1,1,1,1,0] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 2.0 # scaling of Q-function + mpcParams.QderivU = 1.0*[5.0,100.0] #[5.0,40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 3.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay @@ -61,7 +61,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs - mpcParams_pF.vPathFollowing = 1.0 # reference speed for first lap of path following + mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay From 724618f7d939b1f67ea6908f49ef7a4bfa291e8d Mon Sep 17 00:00:00 2001 From: Ugo Date: Mon, 16 Jan 2017 11:10:06 -0800 Subject: [PATCH 111/183] Video settings, 2nd day (1/16/) --- workspace/src/barc/src/LMPC_node.jl | 2 +- workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl | 4 ++-- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 67cf3a3d..c7ec9420 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -72,7 +72,7 @@ end function main() println("Starting LMPC node.") - buffersize = 3000 # size of oldTraj buffers + buffersize = 4000 # size of oldTraj buffers # Define and initialize variables # --------------------------------------------------------------- diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 4e4438fb..e3edb61b 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -57,7 +57,7 @@ type MpcModel z_Ref = cat(2,v_ref*ones(N+1,1),zeros(N+1,5)) # Reference trajectory: path following -> stay on line and keep constant velocity u_Ref = zeros(N,2) - mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.08))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) @variable( mdl, z_Ol[1:(N+1),1:7]) @variable( mdl, u_Ol[1:N,1:2]) @@ -66,7 +66,7 @@ type MpcModel @variable( mdl, eps[1:N+1] >= 0) # eps for soft lane constraints z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states - z_ub_6s = ones(mpcParams.N+1,1)*[2.5 Inf Inf Inf Inf Inf Inf] # upper bounds + z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 6c11a7d5..0c45078d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -58,10 +58,10 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.N = 15 mpcParams_pF.Q = [0.0,50.0,0.1,10.0] - mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f + mpcParams_pF.R = 0*[1.0,1.0] 2 put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs - mpcParams_pF.vPathFollowing = 1.0 # reference speed for first lap of path following + mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay From 0ed89abed42553719a0550efb8842635b3d9ef19 Mon Sep 17 00:00:00 2001 From: Ugo Date: Mon, 16 Jan 2017 11:26:06 -0800 Subject: [PATCH 112/183] Experimtne second day final (1/16/2017) --- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 7f1df018..40f6d7a5 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -52,13 +52,13 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 1.0*[1,1,1,1,1,0] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[5.0,100.0] #[5.0,40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 3.0 # scaling of Q-function + mpcParams.Q_term_cost = 2.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams_pF.N = 15 mpcParams_pF.Q = [0.0,50.0,0.1,10.0] - mpcParams_pF.R = 0*[1.0,1.0] 2 put weights on a and d_f + mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following From bf81da26ea725d9d976b3031fcd7645a2c2b0948 Mon Sep 17 00:00:00 2001 From: Ugo Date: Mon, 16 Jan 2017 13:36:27 -0800 Subject: [PATCH 113/183] Add small comment --- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 40f6d7a5..80a29208 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -51,7 +51,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 1.0*[1,1,1,1,1,0] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,100.0] #[5.0,40.0] # cost matrix for derivative cost of inputs + mpcParams.QderivU = 1.0*[5.0,100.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs mpcParams.Q_term_cost = 2.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay From 6d82309b510bb970682022e6b74e3213877667a3 Mon Sep 17 00:00:00 2001 From: Ugo Date: Mon, 16 Jan 2017 16:51:33 -0800 Subject: [PATCH 114/183] Fix bug in equation for simulatior --- workspace/src/barc/src/barc_lib/simModel.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/simModel.jl b/workspace/src/barc/src/barc_lib/simModel.jl index bdf7fa5c..6a26421f 100644 --- a/workspace/src/barc/src/barc_lib/simModel.jl +++ b/workspace/src/barc/src/barc_lib/simModel.jl @@ -127,9 +127,9 @@ function simDynModel_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelPar zNext[1] = zNext[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) # x zNext[2] = zNext[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) # y zNext[3] = zNext[3] + dt * (z[7] + z[4]*z[6] - 0.5*z[3]) # v_x - zNext[4] = zNext[4] + dt * (2/m*(FyF*cos(z[8]) + FyR) - z[6]*z[3]) # v_y + zNext[4] = zNext[4] + dt * (1/m*(FyF*cos(z[8]) + FyR) - z[6]*z[3]) # v_y zNext[5] = zNext[5] + dt * (z[6]) # psi - zNext[6] = zNext[6] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) # psiDot + zNext[6] = zNext[6] + dt * (1/I_z*(L_f*FyF - L_r*FyR)) # psiDot zNext[7] = zNext[7] + dt * (u[1]-z[7])*100 # a zNext[8] = zNext[8] + dt * (u[2]-z[8])*100 # d_f From 505c06234b9f823af1c02760dc1b2cd55976347a Mon Sep 17 00:00:00 2001 From: Ugo Date: Sun, 19 Feb 2017 19:25:49 -0800 Subject: [PATCH 115/183] Changing uncertanty values --- workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index e3edb61b..6736baf3 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -262,7 +262,7 @@ type MpcModel_pF u_Ref = zeros(N,2) # Create Model - mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.07))#,linear_solver="ma57",print_user_options="yes")) + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,linear_solver="ma57",print_user_options="yes")) # Create variables (these are going to be optimized) @variable( mdl, z_Ol[1:(N+1),1:5], start = 0) # z = s, ey, epsi, v diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 95fe092a..495e7406 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -137,8 +137,8 @@ function main() # IMU measurements if i%2 == 0 # 50 Hz imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) - yaw = z_current[i,5] + randn()*0.02 + imu_drift - psiDot = z_current[i,6] + 0.01*randn() + yaw = z_current[i,5] + 0.002*randn()* + imu_drift + psiDot = z_current[i,6] + 0.001*randn() imu_meas.t_msg[imu_meas.i] = t imu_meas.t[imu_meas.i] = t imu_meas.z[imu_meas.i,:] = [yaw psiDot] @@ -146,8 +146,8 @@ function main() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros - imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] + randn()*0.3*0 - imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] + randn()*0.3*0 + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] + randn()*0.3*0.0 + imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] + randn()*0.3*0.0 publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end @@ -180,8 +180,8 @@ function main() # GPS measurements if i%6 == 0 # 16 Hz - x = round(z_current[i,1] + 0.02*randn(),2) # Indoor gps measures, rounded on cm - y = round(z_current[i,2] + 0.02*randn(),2) + x = round(z_current[i,1] + 0.002*randn(),2) # Indoor gps measures, rounded on cm + y = round(z_current[i,2] + 0.002*randn(),2) if randn()>10 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) x += 1#randn() # add random value to x and y y -= 1#randn() From 2b3fa6756bb8f252bf94a451f4eabf36bd17e091 Mon Sep 17 00:00:00 2001 From: Ugo Date: Mon, 20 Feb 2017 11:00:04 -0800 Subject: [PATCH 116/183] Adding a tutorial for date evaluation --- eval_sim/PlotDataTutorial.pdf | Bin 0 -> 231405 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 eval_sim/PlotDataTutorial.pdf diff --git a/eval_sim/PlotDataTutorial.pdf b/eval_sim/PlotDataTutorial.pdf new file mode 100644 index 0000000000000000000000000000000000000000..7e33984dfa2e00c7f9b9b7fcc937ccc55be41402 GIT binary patch literal 231405 zcma&ObzIb2`~8i9AdR$uNJ|afA(GM^lF}W+&?(&@T|=jIcSv_gHxe>5($c>H^xWq@ z_xV1rhrb3UKYRAR_It0jF34qtMCcd+EC}T7S?!JO)$OSWOs^SUTj`o2@bJ(}8dw_H z8oy?S{zaZ%#Khdzz=mGLT*uZx$Ux6Z-++$~0c2}qpksmH2#!&dvYKN+YPwYU$@3cU zMFx@K=~_G9q`E#u#BmNss`S0Fd zaYTddaKAJg;pt@t*QaHF((Nlusd2dbH}F2Pvv$3jB~2FyeMa||u!YyY5oM>UHmusn z-~8qLcWj3pl=9Z?M|M~CjaGhopjv7p%{kAbbmXeU!HH8J;}!9Vdbu+VKSH66)gU@x zggAikJi;7b1z@%8ms~4_+if z2(eQGF@lytNFiv90%PE&=-0^$GOl;>1n~=;HiHqt*ssxp^&x;Qs=W8?N=`AnDw;haUp?erlJuL2HDt^V zObg&FW@Bmi^P)=Q)M4ISpq|xWSWh9M~YcRuKK|ep%ol*2)ty5JW6#%s2x?&x2tZ(>2AnSW$Ghg>m z2hu7ZwR}p4LyKIQF1vH)3S*5W(;IY8@ofkHMnnG`{#7VtOO=GDtj&_Mv zaWG{aVWg(A7nihhzGJ>}ybW_pNaEzGK=*TZHtkX2D^YV2^-itA8U+M;9TzQCg}z5O zk1f7Rry>|w>i?cu&|7zt5#eqQ-rZp3WW2lg{%>smnW>6S)&}&dGPZe^ojt)pi^FJxeEqGup4DuD3!HL#QHprI%_)qRWeSydn;-05g7kVRPE zfvP->1wH%u)9Cuwn*2}Y7~rwJSc6{Zu~g$`h>&(^F^Gh;2Ialhg@et0zmK@Xw9oc& zrpaM%FNg&-b)Trdf*G_vFkjb_?6Mbf_8x9O0!52-_enf$6FSBtn1>(SH;wPWPabZ6 z{SMQ?`BN^fEo8=x)aBB3^-;~!`+Gitn=v2?S199Y#Vs{SyU4JqW2`ECdl#jfs8;L# z=Ka=1!L#whh2HCM^`T?G(so}8Nxi~JT;Ls-T}%v29RI+VaI&h%R~x2{+3(ZSP%znt_Kp*b5N+- zt(&uRfN%4g82TY@kjgNgm|_I~%af=K{9?O^v9mt7v0`_#^2p-LL$irN@((7=xNd+`IrTW1odFg_r6&n#h2ZY}^?hC!8dNS6QzlF0lA0&X zA)UzDp2+;vP70ZOY}Sdi zPv8qA>DjSG33p2FmZ=$)6n!=PvkCdXPC#i?qxH1W7;VswHvVB zQphBSoF`_iX!LhM6v<5o)buUhNYsXb7r$Z!Y>K<*#&?_7sgdoBRL@3&gQ=d?uItg0 zd{R2q7y^PLwZSy_LIj0E^EkP>?d=+?r$&>RVe^??%9lNuW^!8rLBXiv70W%ej2Cn;1mN0)T+}Hq~K#Trz^gB=)~)NAN|}B zPZd?Vh&JJMcvfhAfOw|nMqBcPRbR-B7=B49kx5~yi{|(xeiP?aXcMAaHR3{-yI1vy z0BO|qkG$1`Ay!~}&@C<-+A(;7zHxu)+8eG2n#flULoE;0#u=r6K zOY}t|be=>u-EP`gYAS~$m;HpazppP^G{?zIYcIJJ0k0x-*k+fztIl$#Sn{@zP|pup ziD`%snqC?ezlzDke94Aut@*`d(>ViyRpZ0QK3u(;Yj?QI)R+uPvv>-fDFQ)_1t9;N+_EUF;qS!1mMndNNiH zyA3O5)-UnIld>54{s2;=T!u_khP!yp7I(U)>#W-q6oJnIkwQtd1|)k{Sq$p&gOpK} zjB+s3)>mn7xbhL0542I7%V)%Xax8Z2**>9`Vos2Z2yO%rm}(iB%5G=Jf1r*OTU-2$ z)XTv1!%k?mle2!osaN;e)BIZM1%dK@Ydg7W39YlpBx0s@h1uhS5Ii2fRMTm~+QS*I z0c=H(sl>cF#3`q69|e+9_#n8&8?f4( zZy4Lc*)xphn$LZha7;9Dkv7z|iT5mI{A^!z;lH2`zB#c;32NA0R0wEXAyC0FXjrqS z*8*?*CL1^yY?wTD)4d|~v1vRJ*1im}YcAdf`Z+e)1c7|G&`f&~SjYqRaN&V~#Td!C zqrr_q`mPKPY`o6%OC)@OyzVR920~a@nRQw)U{eGQa(CfB7kipxNHpDG_t&*br$flh zmEHlHQ{SsFPv~w03NVQ{yTQHc-N~|mnvUPxKRj@;sp0(8p zK(P?$i4jq39Gcwd@7XR&hLsRb=l-RkZ>PgBB0(hpo)dcfd=*(N=0wT-l}xR3a|s7! zad5fjVFM1$5bNMEuh*_BF^^%xz$=;yrT3Tn@>=+*UGc#hyrx6NI00@Vhj!9Vot*6Z z8#B<2(3;nmQ;DfiU>uOzj1A%A00JgsC{XZ<@*HbC^Ay%zK!T9*4<7^d7C$%dJ;Dc~ znYPqTnnV}!Jq4oNT`v5J*j;4Hk&4V~P?hJja7E_=4lvvP3>XAUWUSLX3 zfm3pci91+M(TUnIXZbW^)+>p8M^r}$F{OO6Y71S!x7VKRa&{9itrFN5)Z+(r9P5vh06FX7*HIH#vNyGitFs0vAFTHZHQyj01kcJ!=&1ZO7aa??8U`F*O-S{=m& zub7SV6NT3~G@^-=+Ge5_;bFr5bT;8qyuYGJyAjiaknkfP_clbvRkl2i$rb&%M4h~L zYyp64njXa5SC%J=M3%c7MHsTrM?SJr%)t{LS~S=q+2u=o9QfPjB(sdD^h1;{CAFNH zYQj>BPId={xFfQNEb3@aoW+>;6{Js^6q4u~*pJ5!uF43o_D^uzOU4a#*m8H5$Zv@? z%Hrw(N~@oJ#+--l!2*!6V*;~eujtum#pU;~DQm2sYBQ%Ntr)oj)VgjB9i zVjC1y=Kg$E)i(;%a%|);JtH&)^r{gvJ;@Cc=73-szpSG=ius7soI{B> zXMvQRn)Ie|nA5Fs_}h!|^VgjglEV(TTRNh9iVlZ^^f6z06~&&y>NZ5)_!?~m=6YeX z$6-ZV%bMk^DsJUa@tODV$V|sruSi>X#O}l(-59-=taq&{?8`VLp8)G8vugY z0Ii`x1d;XnmkQ+0(>Iy;=pNsV;K%rpniT?G>4VAdsRfqlY)B_B7*u5w^-r-p;yE1+ zS&;F8#X?7Fl{eg0w`m_zbPAD;Qf$6$YrFCh8W-HX0L+H=g(}$Lx@p1u_T4M>a3J5Y zvU4W;&61n8_d8%=@An#OTm}G3Lw8BLFt{MGgzL0X+A+Y?=c9YtUbZ#(%jYrZ z*)R1h@@eOcmE3XNVDZOej%()av~y3(RhBg-{xS_Wlsde>kvCFnz5ctdBTGJ{ z>uG&hk_nf6kvEwKwu3R@4fzoqJeX1AON+R)-xGP>Q^4I{sm%M%x~t3LH}(Q&uQ{iZ z=8@|MgkhaoTl2o{E zS6qD5i!kiuXK%@aWcVRGf~P@zIWu^TS?hD$dr!TnXSy&*3~N%a$fE8C^O({m5Om=< zwI9BwgfLufZuMqxeuYi#bJn_Dvz86y>m8Ls#xG|3OEB4hzTt=38Zt*r7@fTzL7QKg z1XWT!NI2w6ccrAA^9U8h{IDY+@?RY#(Y(G@7S(7Up&FP|HM(D%?F6xnE4$hyZ7L>a zNaesg;##0b+3eaqp)bp1x*QH&*UV+fXUKF|aQ#4C7_1)W!8p;ye?cZ?vXRPJd`(vq zw4WP0dbM&207~C-ow`#xUoV);(dWe%HH428ahMqd)%PTZ05OH5Vr+L7$baSa<%`}m-VXO_pXe}ZJW z*0g}Gw=Q0nA)(&HjW?qQ!0T32TPUJfJvAknaI4+^#SAoKQ3q7y>yWwMlNekhM8Vq^ zxf5v@g~_x;lSnl%ki0cn2SluQk)zQG^=w+{Q_~xR%T_y@8x@)xY^NBNzTz=Ch8VpF zDAl$HWr5S~#V_sFE#tGqA|wK_OADT|LSBWzAE#o)S2D+iQp*ga%b8z>^k;uQAl|7KBa>yPr31UfT1n?So2YAN>T&^gZ zJI0&M-wZg9MO^d zf+ul)WL=U8LYyg^F9Tu0Tg-ct%4^H22!Tgpr#HF%b#@;Z?Yr7iT{u=<4*Mc}sL)oO z46aKn9e5U;-Gvovlb0fEL%2S?@;W{bXw!5~j4h_jmu-)M%?V+&pWAmRz5QTPB0izN z6MbKY+HpBM>R#yeE)nKg%Y1#Rt3=w*Z0u)0sxTh$L!cd!VMYPn*qIiPg(d9snH3OZ zkAT=FwvP;o&lpqQiyyEWnHl~v!We9<;6yg^o?x*cd1XV5Qqe^{tuLvGv^xO{k40NC zj&l`vTDPBIs(6+J2lCOM_l?-}L>PAjh5?$#PV6T61e2YD4dDqVxC@2*`SuuB7B#6x zio29O#xq7)qI#VV$H^D``7ff`+<5e!g%9Vxq&{=C?D@q9 zvEXfpY8jb5TsxE&)!Xc}uj@QN?`%Do!sj42x}qi14pXMJ7hWW@8D_40afI%SZB-;Q z5)DGyDAB1hd&iRM+1 zoNdSy6iFewbSHdX5Ibqn`l;t^X<~sNH>ake)vqAMnS$gB$l0x<=rx*!H|QQz?5cuI zl6bdz{;~5MX_y<_b<}G5ax%RR+(+81hh@%G3}`?c%+pVmJF92Z6QjW4RohRarpn&X zriX}CHS8gIMRp=qwoH48pS}!2h?ccQDpER6Cggl4c@ZFHxvib)(1M8XTM}H@>nIw* zI`gUd1Z!pSl*}wZYx;w|3LJ*BJgB~SX_8q9o_A3?PBKOXOgi4GaPh%2=jk`XC&WXY zyu+2eOCgI_x;zP`G-7c4+Tm}u5bZ-2!X7tMg0Q9IY)PTp81x4%QymP)|mAaeFiDe2M2+!zQ`rzmbYPJ#w&9yMM$oQ4sPt_5RG7;;iu1r2U@#g@5E$h7X?s zRCJ3sA5v}Y(UVwKWwym+DE)YK+fOxA=OjA3?vUoUO*QZ(QU^<@>r7f+5`6i+qb?pI zMZlyccVPKsW$6RvlQigz*c)ZeDslNLLf|uNRc!Z?ya4qJRR#tjlg9)ErKkGcl zKj6q=1E~zPfz`+%fj3bhokDL**SLg=e*;KshvpR`FTRlnd0ydDG9^aEb%F;PW=o4} zSf}{mFrHGq>0I|h0mmr{7)D}Mo|1Srqc(0s+?PmJD2#2aErQTl*P-r%&;d@$=QOWZ z^j9laNT_c*=1e~jP%NtE5g?57lKdeKW#{>$XDo{)xO>;7n?mT~@sEDK6TzIx7R868 zoHkZ$3KUkD^kG;+D)hTe%FodpvhTkAuMEN|@gV1soJ_b_fELGCXTKL#%ceA9KCcOH z_9fzRAn>d8U!&GMj03M_XCP>}j4RiFYgp&kxhTx>*TbE*2L5Pr$1CPbtr() zr_Quev|izHzP*VnILTXm7nJgmQ;wJ_JGYSNMx9*Xhssp9Fz1DEC%7a+)>d!*<`BWI z(d^y+%}3$(j&`_jp&6D4ZQEwrS6BsnnM@Qt2qfbn%7J#lY}SebfoWXf<2w}N1M zoB4<3!nL=S?4-x~aDs=d@~{qOc+4*COM`i*B|;W-W&3C^>3y(IjhIabXSRuG4R|O5 z^Y}v`|4!9z92q_Iyjr6-2#P7MT0IJEBgbm$2#$5xK|!yYd&gDo1tTOGC?`^Pp{o}+ z7jB_-MO8=haMZXqe<*ovO$hPrMtth>C4D`z5X;`H1~MLMn~9=HRcMB@7cixv;K)8G z9!EVT#QO)HVP~e5P)GF) zSX4|7htsX?xqBT$Em&PWx+-$3R1IsjXMEc?Mu0SNagnQTu$o>Ae%JEQF*TDs@9167 z94_@LEbjdYLHhgmnsQ2X%z#~k3!IcMlqm?OgysO6H_K_0lA`m{kbaArzvrX6lVAP*O!+C}bI{S9H2p>Q`9&1t zkpH5%)lAoI!@d~|Ty$egrlcW}M|Ze{Q(TQvVEcxrrSKBd9hW2jhs$w`C$z3t+V%_- zY`+WKEw)eTh!Gq_9d9;O#|U{P!CBVUzg$&~>8$T{YM8I0185lc$_4bH46ekp5b#q2 zKdXUeLB_*fUX)`2U%ZM-k<-)jt|yky?_Li2s9&E+*Bj4XnR#O&_d8McbDq>>79sK3 zY9e$nDE#_7?O4Cw*R476o`5D$Y|F|D3ENsK$HS12zVGDO7qRE2Iw_43%AEq4p~pCFZQLvZ>UZU_&(DxaNhrb|D_Vpt3fX zUvkkK5nR-N5NlwNY}T+KdFuxiOgCK?gY-8u5UwU}8-8t<^O!b@Wv3`9`r=%t1Usv* z?{DeN=57YA+7hvs{j3NL@QN=o78axb;@TX=x7__q=^Po!9|G)!=#Ojk%vm28DI^QXo+Z4@jezQR(mdwf2{W+lHk~To}#8Ozo*=~y)%}|xO ztF%%^SE2Bv-}obHn_!TJC-uOFCcAbl!sTxY|FoFQ^#I4M5m9+X)npLepjQ2@oY3k3 zvAWZG&!y0q84pXAeJ=DD+BnZ11oEf3g4IpEVyIPm2s|YvSaA~peWw=)sE(}QdbYTQ zSbWhRf~+M+Cy@=}&En8URh#kZNF?<3XbhnGOP`y}?Zi7QlU3t04S`U7BA=9{tmZ>AnjclYPhfldGUS zYwi|uAf8>`Zfcz~=B_+j0EcQ|ExmN7pMPD~?r@pr=kUD2HIq1Gc#;bmZ@f8-j7tFgc^?z98E|$%OUx5_d+ewb)c_>hK_4Dz zg`9?T<4{dIL-9wZd_ymg*~ocYEQIsuZacmSSgyXMVRdmyna4+)yf3p~U;7)=2;=Du z;eAUWP%LeIS8Dv__QZ>53?yMTS35shf2igo8%8M@A0Y|GAE{{;n4i5CkYtQoN zL*FXw;k_6$HB%5If6U$6@jn<06%BCgV;8V?Mo1Sov_&-w3V4`RaMQmTtq`Q6_^6dX zmU^DS{Y(Hn)}l3pvF5OFPT2aJS!sjxBILkH)0)O2tn0Jzf7^}>`c6bZ0L9n;!PXBQbz0)+EAu&{CO#t~ zjBc(iZ%jcR179l(uLDXG*4Khe+L(c{s|UmY~7yEqV<95M{;2IexN= zA1@%62Ivr7J2@=Xh`qhn0dLWlv!wRjiCX_%$^H6Tba_F|c=dXC!qyiMg4o>oZa7Xo zFyrM7{e>seiiXVKr8%8x9hw21imC+tU(l+nYxN%vwXzRB+6!sw~5C=8$wNt73^0F zA*Q$DpLf?u{N0Zi1+c8Ns4O`VHGbIHeq!SFWbp?g)jnopJtvW z9wr<2D|(e}Cp}VO4mQje5a+&-5jO+)iKV$Fz@pu|`0bUoUqbu9t&FT|tUMlGuUq9! zvRo?==SHU`MJEgz#W%{Xh1HCUkfJ3ndeshb4TyezIQxS+p5_rD2rgjIJ6)tN!r1el zRFMP3Kt-n9PX#Q|<2-W(nFW_C z&q_3tQh5deJ7Npy3w#XIVk6-ovBT`!P0@q0sN6QW^-*yHfTjdvBMuJevt{1@1#IAh z1+iyQaVY~u7`3-WJ9o+`Eb}$u@HmDH>?I1@-=xvdYK>UuCZ85f^4yEXFN3j~dV-9= zA;Ad_vSr)FB;|K+D+*p^bszE;TI!yd;pOuzk0{+3H)zVBomrOuT!5n{+Tx(+(Rw|d zg3cPJUsRfjYyL1Ju7e3y2wuNK0rHJAR9`i{j3G)aX8s=gaHlKM=RnXg)R6b~U~BT7 zWf%v$dEet3zy@lszhqm{2wbl+zIIr=c~`;Hu8Kdz^CM%b<(Dhhi1I@W!Ij_-zpY4K zfbaP2#ZF|~C6wP70;EInpiu(7S!;Ai{(3Flg^8hdF=WhI#V!qXs9@)*sz;$?+@Q?XdZ4Jg)%IwJe^=jD}wG#rmB0k^vOp`LHV2De?C)(itnBF%j~5sq)m zt9OS7=nGBIEw`uSVwdw}+Na2cEP;Tq^_|XewECKrmaP|JK|vro{8WXQJVvvKs&raU zcBn(B;fXtZcIVX++5`_Z0l&4l#W*?HK+fCs2o}p@X&dqdt$<{}O z7iz!{p?T&dB|mpm{-#-m-A6NEKDCDPK$8P(EEUhw%Mo4JF6fa!NE$CAahVNXycL^y zz4DQKrI`Z@|JC}-nzt3N?ACHNo<}~7eDP!d!}1EwUe$%F7My8Mz=Z%BY9Wh3PI0Kp z1}mF0=oh{V&V@NnedzU5i}c6rOf%YHT(9;uFd$p_#_d5rLw??-QV44bVB$d3n2iy~ zUfGvcOa`RF(hCv)7fq#co`{TEoESFX!VK_S+>by`mR6XNN?Tqz--)$hP4q5|$Xg!%3;W6O(i6 zg+Q?tUg~I0xY5A(gL6f}A6jA;TxO~8Dr#ncl5fdblrbunKzDbb_0`EU>0Z3Ih`sEl zSizbR?1A$~NJSm4&^(1YqMI>;9Vi`X<;u-D0j=Ip;8iZw*n(_jS1}iJHVi9~QDB>A z^zl_SR(kanhA3iP=xyQn&aRV<6~8Lt{Sp<#ndz*1S`eg)sUobN!lU8Kw74tqq9u|f%s}jBAK@=8q_GC zxo$A|U-=EjkJKgr)D%t9Q74sE>r-ygLR-FDFwNs*!?UCU#+V|%@=>fdN!)aKI#(v( zIAEW^tIU+I0e{`{&4FvAAB*|K#RE{n0a+*HOM z1AB~xI~$ON_@X7iG?V>nu*!F`-HMD5hshY8rZ&UB>R~Vpwh@$toxeFl3T?7H2PVC{ zZ2S@GX!+puR=ApCVpp^)egsdUZzF&h>N>wl50iM8gTU!?^J8UhH_v*4Hn0XBD}a+- z2{di{1*J+qm&wj$^C)6H=@rS)QaW?ik#0yg89Mg+J*Oux_Z_g$+~bWo)z(X~J6#g8 zzTU${HR=4L|I7FNUBqy&dX5D0ss)dw*aUvyxv*dO`fxrIIwvQaVLsmmXQ;qx|5I4n zj1;f_-z}MX++82KlH%airc(#=SdB_NtiyeAMG}RIXRBN39hV7|Yh^~4@uhfR>yrU^ z_Ql^V>GTY(el_4t+{EM+k)IMSeDbc!Hvu*teAhq2X=DxuT#ZbW)(vT(- zETlK~6W+(F&HfWoh3<5MF_Z-LeMA;D+s{`PrLdQT{pbBVNbdzRn2$f1zx$&hWBr%E z%JoO`_$rE$z#$s0(8&6$ds00QS&)+&PCRO$dF8V8|Cg!*^Yfb^^-#mjqGk1MUgqO2 zU&Ax=BX*6By8QEez2R*i(UR)lAVA3R1dGmSR1)zwi4X(4E4Ldk2)p{Z*l+^Mqy?i{Y5J(6Y`1L^8j^KL`|iJ0&6?w5`2PV*QDau4hKvuh#DJT zF14KAB#D{e77P|NIabV%Wq4eiL@(j{%S-$bze{Zg6^_@;>AuEl0ldaiu5ycW7&9c5D{Y{eJ zXg;&BCcRg-F3k=Om+CkLP2@2}c)z+j; z)W6M-j3=iJPf`|h(muL)V7q;eCAVzvF0sP2r4Pi+QDCss`5KBWB{7Ael(*dXSfLBI zVfT!C`nIrACEK+8|L}x$8XWIOa1pMvUF|Q8<`LH4HJT;mKN^iBvS|}0vxG2TQZGW* zUry(C-fyQf_IWkH?{w?24?^sREyNp%%lR`&*8Iys@4zAnZKGZ0@q`*o3Lv!g5DgzSfUtSySMn)Qk~o4DafzE!b*V9O}x9kqNpn`U6*{fhq*2eNT^ofZ=x zK2qUm=JrAq-D?~3u%6+2*E7`rFFl5T>F^aJBxFMpg?jq?YgN7RTj$pB&ARL=3k^#% zO(!~mH{DUh1MhRJknfXqyTv8FC?7oU(|DfG9$d)@16_x7yqF#4Ev+&m+TOJc(QV&d zl7jw@8(l=|8M8NihFSf??OD`DJ{63sK?2|?Zv5p5kXRIPOb)nw6X?c&zI(wO})d1sBIP z*gJ*3<^?SAmG*?f@shTZFPxxWI4dkXZ&(a{p|I-IkzmnshW{}^}Y?~kNV(INXIc%x2A|7GB5FyN0kz74KDYtM=5 zyhVe~8Q%_0OY@(7ME_>tjr`%TZt2 zHQHxym?%tS-}!VUjRo7Q83flExyvsk>g{{F9rmQ^@{8u+nG(a4`QQ3_aLDTM}GaDy3_) zKbp;izMUN0ShSH{c+b8y64O?1zwGWk$m;7bRw%T;5fKc!iN*O7PaA%Sr^UIylfT=N zx`?0W9xYZP^y%G2+je7XU2aoogc=8p@(?L^G4wBa*Xr|&9nQN^*z(aJoUF66Ws`vW1<7RF|)-C zwGZ6(z9)QRF^8s22F<=*>^EL}8#?)wv4NfXgiAaQBDgTI9;W6xm1h%uF8BM~2Yu%!U(v82$A8*LaIGc$0N(}gKhdYwXL72+ z7nNUvPm`TA2A3)utG1SZTx+d+Tbvo5hf7T*CjETOnu4l~@3?v*VjRy9RWjw!ELpl| zXBZkJXFd8u;KuL!ZcI&hVTQyjX)?+vjj5Ks6Fxdh8k_mcUugaJu{D~gMFK57zmuVE z0-s{hL@H!QoQL5+63ObUOJ8*0EDBJtzUcn?nPDpLWGOe-Ulpkugz8_+!TzgcZflpF z8up^96e{%CXxOKEn^sH$0*_VQ8b5hcj|z4dw1{eM`DqW?GTW*(t|FVZG);~Uh3Xh) zT=DUBWc(F#Ym1UWhNj%m*|m~Q5+kRkH5Q&d^nk#8!W07*>((Edayx?lX_By+B z90dy`AvlhFXxun%UiUyLGW3zK1aJjf8_#0|bTsoeJ5N~XnZHFn##-G-tq3ldeM&`V zFcnUbG`hLsBq|+#Tj4c#K{l93F-Kh!2C0sV;iQzG*DGN=#fEsrK>Xfb8Nu_Ul{6f1 zuB<&}++@O%nD`khf!K*Jy(w5X$0}!~F@kJ=WZ<%r`F2y=SMyCl3nBCVoX4NGq|YS8 zLz~*&KZuWMC%uTT4GKAM2tZ2*j*vtSl_h?BWgjD-+glc@$Y|q74dSw(z2yM@+Yg>% z+@P7f!wv$^mzV_K!v3ePA|Oop-}@@qV*EM+L5Yb{WHcPpqn+Ipx|emP;|UEqkzWPU z@tn9<2~>4b#Jcy&8&e#(r-hk-w^PK!L0MtAU>Fphs?G8O3a55vR zQq~NcN@%EoXC*w+X|}?_2n2}}^(_JFMRXtKUW7DM#TE?337%_FcKz(ME9O zo~F<5GU-DF$m)~@D#xy$cj2zp;w1;Q)8Ks;?CkS!g(>_;zo#jTzYhmH%Z!Sjsmy6G zhtX_`StQ?67H-R&@@VQCXD2=ZhXp)dr%Offwnx+V?&JB5jUP}DB2-+OJIY#dMcBo-=G;=)q#k&L1wqK<)SR-~PN4RAWtD4&cmdc_)BKT8 zhtw*=@aNdaK?cQ6m7n_7t-8!!vXE~mm)hyG+QQpi`}9LnI{=oN@R70xrJmyp`>mBp&C<3V?!8B!}g=@4N)c^VsV_C^pd7u zllH!*eJvF=r4hCpOK=HBGxhidLG^4!BotJkT>?1<7JWQ>v0HZl3qNs{)JQfTi&b?j zFnAdV;x!Q2XlC-++TYc}*{^>+Uwre|k|nPFs~3UH9}Qfuh@PL!R&vMmX;d zKh`Fqi$!*x&ff9mw*AoDk{6wUd^M&z)r#vItA^-f*XD(UmoKpm`w@)DZ-EVX>6F~K zQCZE;b?6?dJ4XNt@hD4$>NTWFSBIbDa@V_}Y=}aDm9@`xCcLIk?dkL6!5G+tx!g^7 z5EaRgQ{GE|GVqYg7$xH~p(>a;;>pIli~S{t~cNqK29OZ%aQ z0Bm^RVrNC;JUJ{Xjd;GduM+A?n!iV&Iry2w$GH5my5yF<@O}@L(O0{2JotXf@uca@UHCzFhQZDH z^g!m&%*?yTf8G%WmkdcBSX{VIi4~BkC~;hBKjblN-~GNw^sK)iiOUQBDsg!WMw~W zxVVR}dC_O`l2r>C!|?FrVkmA!v?B~H`324C6o?B(%0hZ&xJCk26p4lR_DqF7cu#6` za}~O7`wFnHMhj}~Y(qVZX$?O^?xE=8Yhr9jlgy~?g=u;G>xVdw&`{9YAnS1_=h(U} zDfMjc>k%+?Ol;`0587p+M}9!V@VlxHW^qE!g?w6H-(hX0H=%rtVaIVArL6F%Rk_~T zm{dA?LUOH8{PWnpb03ipE5_ufcFn|`m7+NKExFL(wuUAo3M=KvCoZLxcAL@bam)rH zDm40y|JG&jEFQgd`TcVNdWS7Ul#fa}?k?$gtnC^*R)`GX@1Wo~t|rsDVNgpiaf(ciQps{Wf+?2|dfZU=z7R&`2O(Raald%VuN zKkcY(6*AoUsmTTj0VuTuzhn5ftM|>X{U7}jr_P-N{ZR?0G>w0eHD>nEX7vag5<7XI ze+YnTJ-FX5B^XQXP8hv}H~+yXRHxY;Yi-!?RVMbRKP(JvKZE04$|)fKRq}Vb5f3~9 zzGI&rjN8o>Ph^}tQH7*U!H=L_c$9@WuP3 zfqU^Fkifk5-GLSkvIS+U>R`E%Nj))jabDD*d6M3gLVF-kFSkjDRu`8GW#$ba(oemP<9`aj>USV; z>x~hEwh#CjvJorV>kNzB`s##vN(jEt%Ylz2k3l9uQ6||7Rw!H6DF$9LZ_9~V1}%~y zFh}PMj8`2XG3i23VDjrm9!W#f z4N1@Dka--a#|K)R6)jSVTeVx?qZ5$ob|v!|>H$lvKdu75NXg7;!M@Eki_sEGUgBq+ zB&PyOPrax8u9ym}sg@txQR;brbi$duaS*O$+9&`GQ4oIFajlv%W}+AK=<~w~aOw6Y zzF5bFv}flzm&~yg;xR!Y(@K6*@64pATrijW6Pi5 zl=;TrIHRs1n##|+#O5Jq1u!O~e8=I&|M-vX{_2>r0Sswv2 z2#V|p&8(>lu??=V=|amwEAyWZ+=k|y?Q_4EyVyA8W-?|L5a@Zcsw@#JI^B-8(sUM0 zsNf8zP!t;Pu(!mKNT0IZmQ(BXuWIA(WUo$^IssdXqc5A^hpw-tS4^PD_0K|Uv?+wJIqHG zm}A{%mwFxtm~i=Z@oQ|#gxYER3@UdvUB>GNn@-L7wz9V^(f|wMOeGuunoCv$?^C{j z;4=GYyT1i4_RHb^nE_n_D|a)%gILjR93cNO>wv`> z@BW5keT;#0ZB{r2M=hl%O>w5a54}45yaJpc-zxZ}rNX!MY1`L>$irmX@Dt_0MeC2N2>S;w znSQik=an?A*fZDsgo{mJ3gnGmx$1wP&cTk>XmRm^Rl9osWp6%n4X;QMr5pJvQ${#w z4!V;!{hHA$+`uqp4zTGJNk#rWc_Y-p;UmZ#s=>2IsD6Gz7i46^tjh(6;y~$miNIHt zn;V|l!8k*<2jw3-t?I|TOl8y%+3dS8^9JA-RQzP2t$f=nnm9gn0O>H9<5!fb%?gvK zw?!|~5C0~Y9sf2l>D9XyL*0pW!XC-FKNmZdyg{hg@EwWEjT03a3cvkf%Dd(GIU`p} z#}5X!lQTV&;l4a(ZP8zJ)VgRWrk0(kLtukd1GmAFR2LFHesyE}C3Bos9*EUsD&8;9 zri8eLVsXol5Yv_OR*x90V0GO+- zli%c13?UtA;O6~acKgB6P*m=`WAI%n0n{n(e3VAUhlwkQf(GFyO8+~66ApPjJWm$p zOFRfh7A>bU90%1hoOvXZ$N0s=(_TOA_>*ixdcpsftoHidmd{8l{Po)-wyamv#&6o2 zX&xRA!=S&30o_|Q&TGv|*a8LDE}wn~$8bj7Nc{jeh-!Z!7w3 zYRy3(=GfI|vdoo%jOi^7OlXVE9m8BHaUcA)| z-~B$2fjTx+)6EZ3(6R`!xhO@G+cHleI>~6JelAUa_VP(BWsQR=B@N2qfFkG{!mtHV zv#4LeRGMoqRlWgY;(?;grCu>7w7?gE(;ZMpM25KkyJ>-N#9as9@FIq_|oDM@$N$Kj0gnKgNZ{a3F zW_DFj^xH;=2GqhK&(xQSoZ1GWrjL<9Rag29S)7K%T|Nv=l;2s%ScX-tz^eXZOX$^; znLWW@?CwFS>SXk)EGbtop%Q~gUwWz0&(eba3=JmtBc`W?g7XYk*z1-L5`;NtO{RHS znUb@;%unT`KMWQ*8Vx6wyi zwpEWJGh#$n6u!iSw&{0TF(G}O$mta&Z!s?bs!SNuaR6WYV} zmK}Cd5=DJfaGW?PuJ9Jw*drO07+ni6scI|#rbB`+}6L{5oG21tvz(vrNpCUjOMduCHZ`46^Ho;B+iG|1hN| zx&d)af0c?zODKBClpu!7*71&}FR1(0=w}rw7QU8h3au(zBau)6`{;?b%t0U^#G1!v z$r35#)FyoncmI6jW*X=79gfM!?t^1ua<6x%L;{^Jqks_g5qa}pr!H8R391XKnQl=i z8sJUS^Gj+7R8@Q6+VFaYGbRkEjsfEs!Wp2zO_F4U>>RgUk!j)IMkezdY!QkuFaIU` zeSQ+uK7nXs7mA7m#gFXi2ieVddx3E}RPK^|=L?Oz#6!rp8r z$a^I;8N(lux2@tr&6^DTe1-Pg7!vLQ^Jn%vff0i){9$v_cG+Ug8Sry5sfkY~Kor1o zWsq9H;?w=yWiuG}^KS4G%sDL+1n=516_H48EvO=EK zsqzZ`8UxP@mE&!hz6!<5oQ67$-#w9?tc15lwNUE!+qc_y;c`u#@}7j-uxq4cbnQc* zJeD}%UF{m`!p8@-U0al5TeAxfAgZgRdtFWhDp?+#Jr%k&cU9YtFeaN)B9U}<&g?rj z*Zh86sllW8j_acQt!HOJen^1DL4ZGtI)1h(LwACU>rAixxpHB8F*K!I>QiG@2aUA_ zFM8D)3dOy+?QhAG;iDYqP=o)nZg--F|0eyYtcU=5nZm)8c+8fuGPguHUzV4Zj_MGY zN#_CQ)u{9b^+=*VznCrTAH^3&jnWdI@ei~@{F15WpQjv_zn}^Vjzz6L*FxSA8ssF3 zWa3|iz|0faI(^E=_sSU|3>3QxFohyvY44BAAKHA<@gc<*HdD*)VHx7{t+rmaSAkZE zvtM2A{9mt_hcj+%+O_4yB7%GULPC^fvfmP5HF$eog7SoUmE9MakubGf%-MA{U5XrsDi=1W(152`C4r$=znwMyOctL4)W#(E1Z%7z)K12`HRLL*9MeC~>awHIg1XS+H<@C>=lP=|JwGtNu`cYus4&$HL zMlRwvr8TJ+n^{W#eD(@K2q<>pdSAKby-sX&s+g5J^cZ894h>|vACe%z`DEgA>CvUC zgEb8*i3Z?MV^3p$NEW)@wT9t)n0=t%0=^yGA=~dk zcjq9G9FIvszYEX+w{#O69H{tDCDLitpHrL@>X>ZfybYTfL8P#p3lJrla9W%ogI{)j z1aXl7Yz_trPyz*LcrLh%F7$2sLnO~zr4j>Nrz(^ z71HgXtT*`^>c>?RX2q}Co>^o^6D{K&*bS*Ph*uJoNDQZC0JdV%4d5MZ){LY;3Ac{i z1WQ#rnY=CmReNyVCn0NnI3vAm`_U2J-t#6bA=h>nO#Pxg?7pxZtFCBNW@tZ$ngm_t3uel?JW>*2WAcvi z+m$REupIKYcmKJkizTM^?5bWKues;BrnFICdgH|rFTFvr^`>Ge z>(?HccS4({%OUVbp+ye7fcxjgVqj$wS8DU-OWXP)IHv3;Y*sbZ==Z3hHb%z0fkW=? z$-~{{kdd3mm_xRz{UOkA1ztPh0-A1{$+}^S4nt~jZCxWGB}`_1k52h=>pP4E9R2K@ zWwD~45ReCe-mR34z5@Xw#KU&pslEO5lp%~E6Bebyibl%DZ+cshJUWml(@FYY5iL&| zUj5`1x=kB6igTvBu2dYAPm^60tlY}GdnsA>F#xol`xpRW9t_T7xs74stmQA%DdR(9 zYA)%LcE$j_T)pSqyZix!!Bzjjgc@OZ&f%)RU#NubP-6UTc3_ zENICx2-$eu;7SNm6jkW_0n<@sSIa(oc#dx$X~g4pH~Rn46&c;&ZAdqJ0{;by#|>U! zuQ0J2W1QTp8^aDKY}9KHLRTXfP3SsaRb_Xg`7RV<`zrix0-C$l0j|J*F{(Is2IPR83aS{8_ytWVlPXWNiNe= z3(4xPVfJ$w{Z3I;jDmhcql1-QrX#9@2`Vd$^JkNcC;nZLX$4ayA- z8@+nh(D0dxlM#NZPcE%@8YcYY(dR#3l2{~rKt_~1M|gG>6G;i~=G2vY74TjiUi*0V zZqY?}02rt3C9qIZtyakOWEM#v1NvHf-ntV(oa?sMPgZlOf-r8;QT4bUZ-#6R8)Mg`TAyzK_Cl z`kzMWMhlp1IAo7o_kq)?$;!VmSXvtNK|xzJ3;|>n)cjcd5Z%_0yi_eA07GhthKQJp z4~uDq9`x0DSjn12?Zfu$t6{6|%yQ*#y8S2_9Ge;+zM3a83T)7uM=VYN$yWKk(YGp`gdWeL74Fx_L#gaaMoRXQ~Wu(8Xo`Ep+On?`_Yl zCDJKA<+LSvqe^Yk6-Eqp7I#qNN4^^(qr^;eKt9DgoIDMvVKvq7=lV6>(u%mhg1Lzt zH-VRw-Wmb=RcY%z7aq_Zd8eM_-e-|_Kq=f($u={AEYDkcLQkL{pELLjdbt zW8^Dj-GzG0gZCxjKio3>X4W8NV|8DZ_0Le>+#|hqo$vyQ&!1@C+#)hNfy$v$A^u|% zM^^rxu$k9q!I{0r89z1_=BSkEo4DfiRuX#jnKAM*^bz+$Wz3@)!At-{SQf z{e|N(=<53HmHW1l*xxwZZYTK4;01gKtY!(-t*0%N`sZ0a6R*D0s5=A15vi4#5N)eb zPWbiQ*e*jW9mOrYBqp;o1t7@1^K=w|cS^r{SG4vVWdBZ=ksDiBtJgHI#O5ard@|RJ zqEYM!hIV<{$7@W25A#+XA70~#-X)cxfuwT1+OMq7TWMNzDz)T?_uuo!ns=^SdoEqE zP3kgV<7iwp)S)Al@TEm&&*DX0GVPudNxfKfPm_r_)gZ~!4*sZo9GNq9>JwUy@^589 zUB+L@3zpHN7fDqf^%wr^F%Y6Y;Ye+UQc3cje8ck_7Y^KvWX?j8?K z5j|WJM~&^zRfR69RA}_M_%N3+vA>42Sh!)s(zO4==UP^ziS>09{>cbM6Egvw#|2Tq z+FSG*9@#O-e}=i=Ett}Z$zM!V@#6ar^9C}t7a$p!!kxk?D5wK3)^4;4a|PU-4w@e( zRQV*&o;O=YfIT+n?75KCnfkiAC*$#AWOu1YekR)_&VOphg-rxqJ*pKtHv(gMM&a@G zn&aMCV!t!*;vVX!Mj3y|m=+cnPAAxQ4lZA64$;h-f;3>+QeKd;ac0aRXoF)M8v9T6&U;G3sImR2L~*>DD`CZ-x&S5}YnC|X_uo<%w)I58TPp3`+@-3L zB$1wlXMB1Vc?$v!XTAY}wsIe3unxUGL|<4Dgmf8q(W*Us#G8(ujg5^x6o0{fj1nN` z_w5;;!pqZK%U`O&K-W)~HKahtz1Hwxk{lH@K$C-b0e>X(1>tT1Vjggk}W5aRYZ$ zlba<(yabV2Z!VQ%%p?XsXY+`_G7Nf)y%o_@WFyHMZqC2?sR^Oewm5|RaN$5iHmGqE zV<^syQm%~@A^N(aI>w+cjW1HXBCJWp=!Ocd`ePX_k3Nyfy3X2=&Du>19AE?2Y@&rl zmvi%pFZ-`+X11VWX76&hK|(rzeCqp(A8C}XAU7y&(--*s`>!#aGoRF|(8_qj?D>LV z0@I)mr2XMQWo+5ZQMPBP2ka)gPnP%rcHO^8MQWcE@Ozjz_~YyOJ_zf)E;~y@iOit& zn}s9j7T{!u)~nfTt;JzftrJpO0;dD?kX;wzHU#!LO}zpK&wjv>4Iyp)EF3DFHnT%- zg4vY0(A_ru%9Xu-X)%3axStPwDOQxGyy>1?@fwXF#IU}EivKFO7v)Q%zW?7&>Fl6Sz zdk`tG=GId79u=wx3o1Q}Cv3xZ!)^TYk9CBcf)RLddp}gnEquE`&LBEUK<;Dl;_9d? zCvc9R+V)9UZRx|-GHf9jFkC`~KaAjI&_?vPOvUq$gVo6sQR{e|MYy(FQD=g57w(V#VL&Cy!&ayFlTskbTaIelx8jSMlb;4l z?nL5P(46Igi&J}HWNJcOKK%7FFF=EF1t*gT`BDFprekeN<%L609E(8S>wj3h!?znG z{^jh-=Hx_O*=8JMKUHEgaI~UeHTvXewBC>ze~buHH5IJ+GlwtL=7Ge~u=(|0Xs9)N zmf+t8>Nah)#vE4#a?|tKTEZZu7y{<(-`l>o^BrO090e?$*o9I<%l7WJxk5dsYtIUj z|BAJ8eEa%FWnzDnJI$f~*7)fDKG?gx4=R8MZ!-5x_hEs&Y=uW~&|#!8=7LES(qd?H zApE4|qP?YMPko0-qwzZ_5$Xdw?4d}cS={XZ*ci%mV^jd&W>lVEXoBEdH$EK_wp4W2 zBVYofma+3__F?V={-{uGw5?k=1wb%Z4AeHqEo8G>&PjT73R&DODbi&O4q&l>@>nb|H*WztTe9^NO zmU+0AZXdc)LLbo6%XFVLq_;w!xG|cn=n5@MgezJE)7g00tIGZ4f4BC7Yl)N!BncBA zun4T};~5#N3s8E^wKv}MLiSUg!*pY8mL@$f(x zRV5cII7p3b(N73ThYFg24~d?UhGb1B!H`EnCZ6yyJJL&^@hL(Z%&?e-0ml`UU_j?@bWAQk~Nn%A!YT!I>rNC#c!&CWw z!bR``E-V@~ZfiO_bLAf&(7{f8 z9yh$QLAAZL=Zp58_G+^O=L7hJy*_BIpKH3lUi4e7KSx|;N|w<>L{T&35irau`-pv@ z+ddoV@uh<=_G*9rV}k7Y-d=WFjFN24to-Mh0YLKegy#4!YVYOl;p2YN;#8CEPaT5P zu3rx4cs!kTz1>w=gk#yitO)@Lqw7Mwsi#}#l$xi&M$noIRGwzrmY3ml^7auJ8t1Kz zXy+paKH@Ehq5zSTUH>-vKsQ)I3AP&H#@#`)B8}lt+>*@&>$F2okF?RrGq{8&=6VL# zeEL*CKSx2UwFIgRH}pr#++P0BQ{4bnpz?x@&vf~gg$u}&-lVv__qwGA9d66?1~X^Paxd{jRJgn7@- zfc@EpW3myN{VrwZ)#$M*hDImn3RkQ$BMtN?1kfMn@!GwE2z)mw*PX5%2dqmX_lxEa zD)|kqKWYZ`lX}I+txLY}6dD`ct&@*A+zfAiNP4{%mTf0{uT?--jehA=@d9}p4r_&3 zW1*%T-6JcXv$jqa{pifbgy?w)QWs_K(}{1>A1)cJ@*`XXjwZZ0d+nQ2Y7E>?^6%`H zC*<>iZ*p;LQgdYDK2KIX9rtObhPK7L#G}OdQ-Z`YeUH8GvHuwctQfcHS&C|=o>b~B zQbXX|2$+clejmd)T7<$G(p(Zb9OyKAypUVuSTgBLt@2{Rta=c1g7}&xcYHndFR{=M z`j=QZ7WKUYkdvQkL9A9^y`)okd;My|DDA)Gzmo^~?`j|7R{W#Aj<(nX)W3a?pTJK? z$3xMJ(f3pYt2yHrluW{z?jB0^!PwWed)Qhx_hc@Y;EEKZLggNX#`r8aWnpw#U9=RQ z&C2eZefQ^Ffkv$<0}%_V3^dUUJyHR;`vrN@-GY49@(lZuzOA%=S$=N}PU-D_z*I|t z)xAd(6kB?SMkCWSK7q?p%H-ssBzArl8B%xjliaJ3DD7_N?FlM<_fRY%v@p;l!I(By zJtK5%nz=iDu0C3J8FT8zK-pM(&e}<*a;P7%EjL(XD^eSMDQ_pPDs)&BUV6~f334X4xHZzcv}R)cO8N*oSbup|4cDJo2yF+F-jRN~#D zyf5GIAKd2kA|BmVy*Bj#sS)fkefR6Pv!%3Y3J|8kqb(cDd--!X9l1!@U1PJ7v6U@i zg;l^`9a@Ai=450pHgQ&ju>Nl zy@DH`vo3zg#ZAu$RBE^9@(|!7ys?0Wi*E)6Z&1DF$gv@%6WYs3M7tKw%dq7-AOioZwDwwjz@zBu9{-;x9WE9R|A@pq2QqNHT8CJ@4G z|3AH?t~KNvKIyfKVxb@fey^!hO24kG@a6R;WIZ_ZLIiX5cz+mJ%b=;GcOk46G6#Pg zL;gE&JY!Az+t~7@3)NQy`~^Gw9L{=oJ$i%R1ZWVrm_x7Rx>a8|B(Muv1xW3)D0ju3 z{Si`lULs@X-u&K`W!*w-Y;SC)d3`IMB+q#!lV3B9K0=wKm~i zFO~0|iL?~#v|D=<;|8x0JwP7L^kw~J%I`pXBpGj2!jX|d{(&Uj&>sOw*W$a~lSMw! zn>%5W@EeWkhxjMt-SlhY9E=8i6M4B`?cDerbn%7movLXY6-R(xA|iFDR<7SXTL_42 z9yzK|JyI7b!f{O){xwR`pFWW5AcFkGx^AG6te(PTD>XlTcgFK~{?#3Fe)h_>+tZOk zI6C(@%?y9&33Arn0;cM0JvsoQt5J5YA%RvK1<6N0E>%Fql`Z@qV%NYgo9puPdz)+S z^_)YRgxU3;G&rv{_!=7mMPP?K0ovpn2`Qn^W|4eeF(=edIVLFBw*gAaZQTmI^6GYy zIQ)!!&LAa0NMBRsjd&wbqv9V!Xz$u)O_)K|;(#9h++(xS;cyHxC%~5-6{7L+9_JU` zY4<<}*&6Z9LJL1l`pYp}c54%QaCKDm>@r<%SH^*EwzwfPMn$n9q6NIn1scOVqo~Ah z=u3~G=^X%()JpX)fIlS2fkQ9-;>HD)J$M0G++K{O@F%5AzD~Zm0i77lTkFPnmjR(7 zhgpn0Gm7(%y=+|G#&IUndbELLU|PgqzFeZm=shW#yHN9Io|w9*AIuHADvn&FkC3a^A{nNDPbk=xivcMS|&0i=rXF(W2U_ z@3p9E$60I8^=BXxbsNt{A$`-*45%2ui+!wA2WQ&$2u*94D3!yvSsv?DzsnYsqM0Yn z8(KG}TBk`rNr0c{#_~*js2kU+Yj)GXvlDP~SY1XB{v&kW_Z?GW-oe*9(o~+!prB%~eM;*mFcEu9^3 zD#Ne0{@o>9?AfIin`fsO@SsrQ;e{g-!>=~H zwd-PU`2jF&dWd4Ck$YbWZ=uPq-{I&}$VlBU4t92;mOuBWl_?krfqFj`{UvHSbZ2z? z+R_q@Yys-7^Gq3vNo-dnC&GAUO&ZV~3ETE-E?r&|$%PtY7Yp@Rn;1Bp5`BqOFys1S?Xv&|@K zLM!OzuAa5y$rSyUP=ryD`;%jZ1im=H(knv;49&BhuvdR^dMMGBQPtO)QOv$^%jxqb zv)IM+|CV+}fFL|Q+Of;H)oG&gC)DJO@*Q2O_)Oum6SAcj2`MmiY4%XPIKa%L+ta_qW zElOr}a6>)^5PeAGIz>Xpp>LviW7Ga6@}Cdbbr1e6`mCNczDC!sft1Uo43TV0$Ca^f z^{yGB>&1{=z+8mkeNfg3{`AuLHG*rQFVV*Z_P14Txs3a%U2M{pf%wAAn19+x^-0Ov za`2jqzh2SN`*}X2`6#(QRFhnQjY$>%%0n2))!lQcdp(zgz{4`2Xs3%~?xyV10@Yru#9SWKG%Lhixm#wi!6jOx^ zZ(NsZ7T?9*$}-OjoBQBtwo>EMNOl}N0o}M;hVk7j8wD6N9A^Zz=g%v(i`^cQUjhH^ z%e+k+4j!PaW}&Yd{l4+0xKPBD$bUyMcd=&I&P^kna9Aa*G<8h3?2pRA4VnXLb{PqF zWEIUG9dJU-;n+3i$kg-`uY`fmUD5?nyXCZtvbT0mqR*|}m!FSaD>;#Hly6pr42B^0x zs{V&`M5y^)?f2I;=J}HjIX_@!`8UC&>Y(r~G8dz8_YcR}+tj6J^{_Rg0?K=nqzpa| zhrbFYjX_(IA{`!3vs690$~|6aB5IBpWnwfL)9Mv9;JTjPOlBETd$ZpU zP)(Ft9X+yCdCk@KzZCkXn=(mWXr4En{FMkNT2x33EXHtP3}U}Klj zTPMv{NKtFAT0lc?W$XA)S(mkiW~TAOgVe5sF~(((km>ju2H1Y-6gXPH(J6efVStJN zomD?exqZ2|#Ia{dpYHfr=mV#0U{+B5t|aKKDh7NKWz2^lw;g94Z>==j(AW0DLrFj_ z8Rs|2*!9Thy*zhH$Q}DV+nS97w}}iiFbIewJmO!XMD*i_oHrQ6Gd}9Pu8pd`6zmGK2{m2l)T%R!*<`>* z70B-z=3^wig7uzW`A(F8h<$U!SwHO})iwr+WN?ySy` zm~TR8$E?;I?0cc-30&%|tJky9h!0e|bdznI3(ltryLO$kp*XIAT_c&HNlrO8&0_tF zVwT_-WpUk{QX#Qaz3`ijIa&y=RS}dHPcpaPjLvJYA*OHFtPf>SP1>%QGq|3Qj^_A8 z9XrMxH>pYBI0UCVv$AQ9^@Kq*1lI?pgA>bKoGsIoy1;=d_B)YN`<`!V8SKup%2qTP zNBpRNLe%RRcO$(F+4KFf(icrHIKg!r;e4rwXo#d!)HON-ycIq_bHEp>7e)sdAgUIq zpVJe41&*5>abqQe4Qve@gnF83^8=sFg-<9LJ9`w_Z_76)tN2$2o&y)o8;&j>&R}EL z%ZNS)%9+YC7W2&{iB}p~$ExDw3rO)-v@`_W3YTfn zY%RwisAhW?GDeF2L5^W!{hd*}%&UG~+}@UxK2;NBlDQA9!C1b`^2+!=;s*Wz)=td>7%R`9l^h*J^}&A@SdrrSEB{ zZ_GMT%Wr9?Jbim+X+7FyDwGH)fG;o1lD0*IPEMpF^Q`ZpK)$}SM&{Ja4_!NNs;E)n zdNz-0cHw({!?TRD;6UzAB+>~n(jO}nP-95<6inrR^|Q*AZf+IdVCG}%-_JD?oX*K1eH1;gx<# zEA)d8(u$0tuOTW+vD>`QnnKYt6tItvg#J?}0f$BT3wo830f-aDWE(CbI|K#I zVqUe*dT~y9nP_wI#Ci7-%Hp3o%2%9tIez8*Jm&V2F4c1b`3}-ra{d=0TCADyl)$yR zk(&1QaRaE+gpm2&#o1W{?6dQ(oDjRctxwVGa=LlWA6c)1UMnDoP4;O~qFJzDj#2%t zzKXb_FXvy8+%1#h-#V1`v7aK6)4mF3N?qS4_bqeC_)Tqu@Ka**A(g=7cL4lf0bs_`Z33?S%?4u+FM}$OiDnByDY~4#*A00k#%dEA zqZDK`IwD`gjp?D)I+|2G556$Fr{T+qzUWkeB)4N7Cpb-I7$*UK=~a7kADoD#^~}SN zsZn;fmqZr(S+)=68K~26hG}}S8#~bAy&Wiv_r?xn8XSb`)zEd36>OEm zlx*reVqshPVnao0d*C3|9r=iJYPOiZNjGqnVPuGl;qV!ksv9dd1>>UFj^L0z(MV$P z=lyOv(duva+R8Z9B&hB~j?|1PHhcN%Lkc8WgdryXS5QR#nY@2&NXabmmD~TrMzl)< zk61kfll8jiJ>Bfhb!<<}jq-|E487G&XjMsNQ15L-ehG4vv_=_!+K9+UeqV7AX*@0* z$3yj5vHZ)(vuV`uKg=__}#$!JdyjUh0F?q#!a?Vs=;JNMLWRd*vatZ7)Ybn3{ zil30HB=`49=nu8g>+YU7t)#$m)qqf5dt!`Cne3BU-1~-eZakm&O0TyI)LT$ z!WLaob${U7BXsKm61uyRf)_c6x5l5X@3^z?ZyUpiTW{EWPIFDeuX~_*%Qt)S1+y;R zr;3af&|emyUPLDHGb?9Fu^my zT(H!{rGNu$TM^luGi$tv4y6HjWn~n!MklZpY(R9H)LSe{I$_@4rwQlurj-n{!PFSt z8|KrEzGW!~SWQ!_1hX(Oh%RM*={)OwKkM`peBq!Sr&UJD%H_@^!ig7j` z_<(pw-2F3Jw^er3Mx1PuWc+*mB4mcc_E&~F@3LOjRw8^yi-8e^U6aS)0X<9pgsr6_ zX2dfRS;d}jhT0<}Zn8Q*QgQ$wPnA;x(^Z4rcc*;G7J7T4}s)$DiGzZ;hF6kp2hu%mrq4z+D_7&M$aP5EJr4f zi}TkKG{=`Cee{em=)A|`|4?3Xpy|On4iB+Qf&hM>NoAJD; z7Z4sR68gfhGW}dNe^o_f2i}+R5e*o|c~-XX$UuApaMU-pnqDF6FI%zmE&)S76{2Iu znH1PTNLS5s!J@C+r7!^7Em|WU{X#zL^>Ul zu~`@Byj-kYk&iDe{sN*^AWUcS-S{(wE|w?by8il?k<_Hlb9uWnepXeo~D`{3fEUD{S(i ziKR&SV}|o4qRA;zhNgFRi6^*7c zi-G{#N4Am%?q+;|8tzfKHW2V)TOC(apYVne&z6S9K%f`Y>k9&P)W*7oM|n;ar+!vj z-D6+L`LYY~M)mDMvX~Gog=&piMnDuexyw)jN=Nh`?)Fc{-Usfz($jQhOdE}_y=u6N z(t9`J13ZCvCYoB&c^Cwo1$K#fm<5|>>X2XSt2|tddqyk|o`kA=&~t zc$*Z55_K@jh}J^lZEMPasCxXXD>Cvu#7EtS0xVO2#oA$KlYSE)+@`-&NnciJ!ay-zpQEb+F`1iJKtm9F!so^%sy<$a*h;0(iko^P02AAZd|Kj`pQe5 zbEo4N8vRAbF@7E;^?A;uT19ejn3N>{!O@b^{%=Ri+R>66V{mn|Ac<*OuzUd>EsEBp z7kwB7YNS{5VkjLdM3`I9+FR(iXHp=g{vo6feqgX(-Z1jnT5Mk3&~>1ms7t%tJb}>c zlFDLS%k$S*>$wPvFvV4}4|%g1Y2~*!iT}C0<2am@0+2z+PIh#Z;!N%kX?A1EWUb?&50s@3uCylJu)E}#a&EiBs4?;Op3uu2Oad|N!Gk2&r<-4txC8G6nh3)y02{99oye?^YEP+jolx%DPtiv+ z>P;_Exq3*=ndEZf@aRX=UpyrG9ycBmg=Jj*pP?Bz&X5aE-}}p8Cy$jIf2NzvZ@BcD z3he~H>!_4M0|VppPa(d;W~vc8eV@83oa1j>2Kx@j_fPLgHQCcQ;Xbb~*ZAN-PrL?Y z3z`EzAYb%gSC&#=1=}WE)x|9X15_5e!!JSRpXL!B*=X8o)Ay!HrWYyX_wY^5m(9(U zrei)bK!v@PsoJ4U$tA86dHhw3b^ir-xw*5IRbxf}X5v=W&r#8cY3cwl|6P$k3s0KM zC2s_nX}O?geUoScQMaa-D&gn%G$TOPxfxcwK*x#nR?)_8gq& zH_&r$LVjA%mxcJNKfMPWL$}@o0gPW=aM${G`J~{~v=H0ND`7Q4<%%99KK&Jx@&Q$y zu;rIqCx)~+lZ&ikyXqdoe5~*ve=wVd8V2fy5q#tHaaJ92ok(BG?1 zl6Sec{$K3zDretEL!{AFW+s`xqgi=o3K%L?C1&x$D4;~swT>It&ox|qNUtT63rEI& zxKpqzfh_Mr4cQFuPb0bqf;bd1^{=Q$U52z4#cA~u5*H~))ZL^*DIq{e4< zt5)2WX*(s*~C@_wbyD2dKGCRNkl)jNk0&(9eCoVn075oDVwP9&}9E3Y|c>k|$ zI)lyqHeCei#h3psI0gel`y9G8`YMW<>HZx>Z2iphKAiufz&+<5^Z!e-j!+Tvht4+c z120Zw5TrHka2T@QTeuloINE%}8HFD17h-yVZ1yI2=Bszyb9*o?mUDjp9BT&04W8T}t=D=A0gh53@Q#=wn z7w(xzQXLA2liB=A&Ip^ACDEzDXd!gK-`^I)cbYtknRpw3u4h5z3ESGjH7EJrtY)j~ z{sRK8p*Tpq5T8DDieGp@mnJAC&SylmT=ymd#%{n#;|`c{dkKgUl>CVb$svbc{PuM5xEVy zNAu14gx6w5?(fuYkxjO$M^6o<9IqPsLxqz(mnw<0PzD>Xj0C^3I9__B-L2nZ!hqL( z$dykk9b=PHQ)$`*ngP?=GO(P>?R&f=Qkf(Ky@wI#Dd(MVIrZg^sx=$#G-E9tdw#Qv z!vb&4$tCR_{RCz znty}(;CE*VL=UaNxm6#lro3rL_J8c>KQZyHe2$^34M z(Sof1f5m8EOQ9|CwrwsoGe%q&Patk8Q248%QO)xW>ek2iSvA?E91jDYS4W8rxP?31 z{gV*%0POSbKLLgGBT3#}FBxzkJOTsw_k+=21}D#}8f zo5Lq|Y^n}e7p4E)XGA=11G+^qdw;}Ex7`7xz%)qcf1Jm=;u>e{n)CjV)eoWrDLV7U zMhUxVhySg?3@jM~cvZf$^o0Z&{ln5XzwPu;SW<*}S6K42slTn-#g(8SjB(dNam*!W4EF^P5%A9dd^n^Hab;Hfm3)C-7?Ff710;mqm2vP`o-F8VqS zw2@f(*U78ML+_gvTd5$(i*5T1; z<)iE|B^Fx+sCvQQHOWG0t6qP5DVtOpzT6Z$kMaXE5_8=o+X>>y{O{f1$g85 zrBESMGWGuBoTq*GjMDfy&1YlLuO|5EcZ|fFCNvnt;(UGL{hRWN!gT1qg(s#4Q=q=Q zuK&{?=4(x-Df>i~9taFYteKD~^~&AsYXC{s^Aqu7bG1klTJRyN%72{tAAfV|yBge{ z`m;*vq3`c1&wvA|3M|E4=X0aVw?5}S*|e%T_GsWz|6mi4_*X_Xk@y(Gv-IxUW0m%i zISJ7JhU_>M#cra1GWN{?1M9F}ZCBgDMBf-AYD!5#EdZ`_?>!{lzbAo1vd@xnv^OVu zE8ES9E(6D~hu6%)iv?Nw9y}I--xk`FIn%W^p16IdivEvT=DEPx3$4Gmwh~!8H!6m8 z2!xEXWi6~XRw7N2I^9aKj&y*B?G4j?`36|%BZ211^PZ!#lG^4|;!$NBT-n%9>+|!p zJso|Prsg5cAU=$igT2eEadiPB&pZ8WNBq^W?qkh&H*Knr)^I?Z>IX<^F3*4Upn0ZC zs0Uc$#4}%$F&1?B5WPVEnq^n+Cal{<*n8ads<&kwXa@NvV|N=7#DBrN#Wcnm6gJ}X zV41Iy;*PgdihatQ~x2(hEGZAc7 z(g2GvYGLD+9ov*4Vx2|E7MVRIk`(fK?f}mG>O}%xg2FFmPd-88;|}nWN0jN1<~u%q zpVM?+7{0r;JFOR@ptrEp$>{yYJ;jKF@GzB!gxA4?e#51qJc?N5i3`S7tg@RIMWuc& zf04mPnRWEVIwU_sBCxtm;1FH8-de5DH?pF)BZ8VJ^0x;v|Hc-MaQuGcdh`Uwm5E@f zE1`j*pR*9t4-HC#J}$JE2y5Y?>|^}(#C|A8ALtEwCN^*|m!m@8_q}hA;FrY}qzQQ~ zQ8yYqiH#NTLU{>0&S95SC>WLZFD)Wji#+~`W$(jvP(T&p^MqRk+{9K2vlc?l_1n;WNBd*Ew6Wx&1d*z-A7;|T0}?Pw4+8R?$8W31y5&hFJVYK&<}`T1WDcHXZDPZ zoXH&#c-#~lM6Vm^51VBg>OB7R&{XN)@dW30Ts!W@k}-LIUJKfhX82^<}!P3ip_zPNnMB{A%9HkLgL?iUSn$JG*Y>iM;ZK^Q}JQZKR3yh$- z!1+C3wZS03QVUV;{wiQM2FB7{SzPD3>4sm_Jf#AQ@enHEvzxge0ai_c2DjSdx$x(| z3e4KWzi6`Tyl2V0viTOt!^O(qithqO3XxHN)?nAYdN)(7DxV3(YX|EQOAG?%y(gSQ z`A|C7*g8)>nD9v&-zDm!HWq{TFzZ z=}QZXbIolH{6u5W`OCNTy=Vq&QXOp;zfB> zo!qbXE;Nns+e$X)!2=&AnQ7hoez3R-;rD_$L62(LtpAmGuzLKEZs^N0x*__ZqJq*W zwWRZFO@1UfD&6eqEj7b4y%Y4eYT$KJhGl#oIZtf_gR-lXkWj)>v@kC%ZOrB3b_qs` zH-y5))@CYKuXT*4l48^qhh9xf7Nt=%$t8K0NPc`ER#P^L-WIQ-H4;3eC@daG*QH)o zV4=i^A6(JJrX0>0uKR~RH*RFQ_T}sG8Mi2u$B3uzLR6wNFVd!=ZrH21-d*@mATwqC z8$^Wvk1k&Gt4+ybA@1=SPyFL4shODFz4Z%%KxT#isJ?(ndi^J$gS3|5HNI;f6l%R$ z!ARRrR!Kg=VN`cJ4V}xvZ?^1*RxSdv6$x`;6@jepM!y5JjSq2>jyw*cC$B=5*@Ekq zY(W1}34b6cdL&G_jACRanIMeQWfg6tm-TwUPS?SBYAYSF5Jd}arE>}$^c`wnl+`9A zXws5#!(ZMoNc1QwEx3(sthw?GVzTfRJO0IWreA4jl*sQ2)&xSo{C=#_J9+f+&3fAv zU5ukwPtSG3tn*0G%V7t{jp>iWhF{~W$xok7YSp;WKG9~^^#x?-S@pXcSdl=xHxf)Jkf&Bb8}vV1`0B8Gqh|!VaH$0) zN9gO_%C9}BA_IPn3Xk=Z&J(i*t?TV+g!H)A4=l4R$sv??@0#_!X&8 zR>`a5Rf*iMVldVCOa#M=%_l-x^p2bfNw3ZBbcXNMfX;xb<5-7Yd)lIigo>9tUQ$2q z{`Tmlcv$a4$64&~q0TRd`zruz&o>nmP2Ce#QVH-5ZRjlC$&A}jYnDOv@?wVJT?R(y zrdA!aw_vyOJ~z0PJ@65Y`E zt?(bYb+zGg@-BxU2~tV?gg?>}xgO_TF6URyE!S38o^zX9JMecWSu;EZ$NtU?N#K@pZnX{lTWw?a$w#v6q_bS*|6}Z|s(!)NdF>H^5S!6~-1t@P?}C!01HNqWnaE(?P=DeI zymYHFmzUk+bp!MyKN(+l(Gzy2bn4)%8v1QhFDg4)ozyyyLFpEUL@%N?wC5?pKay79 zKgJXVF4jZdYvDG%0E*rNrG7u1Q_@+hfz%yU@xV?v}$ZDqjp;_bT!j;1RIb)##U(fkw zv_d_;$K=AP(<^tuivgz9r6;Re0DRvOv9hLZKWatO$birIZ{bFmX-^$6j&t6^jLijIEc* z@6|)sVI#Qu7Bs$pVyfh6T-zI1oNMJA!JHBREmYWaf1>7l1d;?&57c86-SkfJ8-F&! zG6N;#XdRworpiH2QemILw4Qz1roY?x1LF5mlzXXs32K0L#c!)TA?l-D;DFApS767boM|{@4v0E8T-+KL*m=nH4PHU3=$nE$+ncrQ?6u zSuYb7C+BPnPIF|cVpcG#LqZPjAq_#`1YDqylbr z#UInbVz?4f!0g2tM;`GILH%e~_SO63`PrTC`a@dphhV$^?8brd>61THBKb0?{*%63!AHsA+PJva%W6~H=*?CL(+6{NWAdJMX?gI!a+}B` z{~aD}e!AKT{fG>5HSgBnMrUKcfp$AlZa00qu~lUERH3BOcDoqJjI!}E6cQI=6hZ^L z>|Zy(nblcxn6LN7lb_*>G-Wy)|E}E~k9>DuB9!8N%f2S{a{AKD`xrH|LU^YXBV(

iSo7p#{BY54KB_369eeC0OZ8#b{=qNOsKw+F9_T3Z_|6r?~P071ajXcWR(j1 zneY3F{HnZpp94M6qs?@(o%fB!*!#wQhJ)G>mEMbIMug$BHLE&)TKQs?CfjYzf~W6G zsAk>=3)9)cK}_dH&aPVuKWWfAYas|f7htxulh6QH{`$|DC&TVQ10RA%5YNvheIu?# z5B!aJdV&*M&b)bPVzx|+5J0E2?R~?W3Cbl#d_N&HRZJJJ`PV!2x5?9AVrc5oHhR9( zI59Q1NBcW*Vl?pV({kHlOY`_cIZnZ3^cpcQ4GRm?@hfwU1pq(eHS+q;6ge>YOW=KT zXm`7o`BY@$A)MGM(m{0$A!2=rY6ytVv`ZM*HWnql2!-^C5`Gh-lRGLnXZjt9SES09 z%;^MW;*vwPM$xKfq~eQw%8>%~#rcU>@^k5UZNCP7?f=ktguXC)Op+UC;#Mrd#Pc|9 z^IJmK_~Q>!kZOEADubD4$NE(tbb*Ol;EF9(#A$e|i*KzW#18eX79tLfziu9ez!M?V zPgZPhHt~ix32?Xn$pDt8*Mby(fJ1w=D5Z;%pRH%lb-en)(ipXs?? zV##?wYF5EPe)!zscX3Tg)=POF@(nTryGtxt*xAs+xjg>*44$qjEJ-cs{9>>%JN zj;eD+TsT05CI}@An+>|_t=s1$0fu)=VZjr%&@eN>!n)Jo9`fjZ_Ev((loOx$;kTnehXG4WJc05`E>zCh z^T+9vK1rmTV!GM(^%)G};*`H_bMjcvP1NAp1pgp~;-58j`gg{FC64eP#YkKY>de?y zOhs981|R%&3qgrB>a#{aSPQ`tMo{qOt7zhHdWPI3Hj*QA zsc{HS5uTGJjg|gy%I&gM`p%#SFgS56331X!TB+sWH% z)W5u`b|UyCuVhB>-Pia-jKze#xZsma@+qfs(gyZmKUoR|VtFpuUrXX@&+c}gM*|SI+kwMrKb&X{)u`!u+2yeKX5u;~xq-ckKXp z5Au+$DWYN%-1{yk!)4ph9O&h-tAppwsne}cuV3Reu=-PptNu|LX-1pe zPXc-68uyDdvd!_&1KxPN)a+MJGlpMY-G0OO`)^>2H=srB(c^Y!Vn;Dq@u2tcO3`xM z8%rRV%}r+;Mn4KAhBYTo0Uqt=$$+VG;1qefon>eJ17sdlM%lmmGiuW+Z*!k!9*Cgz)z95 zdY4-N?kwaY{7UlLkbKno6qF-o$i(lOtFqFgd9jUrI25Ytd2QRSEyR4ueI0b)V(y>d z@UxR?iP5GI(Q6{$mAv9&!|OG0DL6^L5N5x2+@uKJ8c$#k#0pA1MtF5bc2>UH?#Z!# zpgerEl^DbEo<7LoM>u~_z#M|-(SQ@U|2kEsZ_wco^&Uq4rCisO5RKMsElK&zYi#%Q z@?iAq8xQ5ddI#vO`DukyF68g@qdhyQ1^W{C=ihaSjCXVfbh`Ww7Ufn^mC7!~5Ykgj zaZ;wjMkjUl!gO>JvSjHu zr;ksNP3rTCsh(~bGw>ZVH+pP=xP@xwr^y}G+1`6}2ceTbPaR)KxL~~Y%1I^2*gOT_ zKVlHbS-|R5MVY@WNk+BcLzw5W{4Qw8!9H>+Xzyt)_ECsKF5#5uYxpu)Im?-YJ~@Og zgTT#k;A0+hfI@cM=S)!9!?O4mZ_W3D)~plj82_vOWrWj;NITbbQLEPOy2~&t=FYwh zUc)g*WNi~bg$(@#*kf=XAh_|fX-YxAKb_drXRz$)x#Q_yyU~L%=38t!m-Bczg}t8F zmFwa6ApRO180|Cu+rz?>+a}%na&-0hjq50;AXIaWWMV_bHj@KpT&gFJ&Hj7l(b77W zN>D1t{q9N*6XIxJ(<2`-=eOT})itDg!*SG;A|@@NA4jJDeULUaMOgi!PiV&yKSgAQ zUY-P)p-I^b*gLLPO0ha$+?_3I)sPY6w|Ut2NdpS))(?+oR*?#=`HDg0_iO7r|6d2m z3m#xZ0Aw>`rzcGx1a8LW?QXZ>f<^5mPil8M)qnKp1{dk>E|Kq>?X3D~lF`WJ9C-x1 z%*JlQ^fz}mM5@oUwBh~LAb^D=ib12Y2|~o$mmw?*8o1In(dr}b`-8~cdbz&*(=xv+ zOkJ5CFbia~fFIrErMg9Xa_m4YFe!b_%{Et2L!r>Up+HR(km-*BUC~xZf_$LjJkkTt z?84$wePT*}E&q9adpYTnDSDCzpPMr*vT9;x_k})*GnaQ`f*xOkwLvY->{BdqX9;@a)b%kpyjF{l6ctzzB z)G}-Q5dpx`?ly)|a;{P0gulKg@Eo;O&sM5#KFlg~tFwvALg`|w_$Xy633dNvXH>GY z^EQY@G@@~;z|-b!cIs;3blAjnr&F;tzFzJwt=Dv1(oa2mgU02E4r$w2=cd_Fw}x&^ z=(YZvY?i$J!Nl#FJVyPvtXeVkFegrhTH&Zz>^$8Di(z@)gpo$lq}^Fd(i17>q4eRO zpe~}poo$sy6+;9Yra=LRpVBuoL|XnB`&%~1aai? z{Iu?VG9|Ep*cXoAgpMX$$!;<^(9T471$Jk@{mLA&VBkbRM|pWV2lUIA-I_bvOjU7h$r0X- z>k>D7A=+2x6P=80=a%UqC5fy1ltyTaZ(TKRh^izo`A|Tq>6Ib99gOeB$wu zlkz_r!7v~ zuP^Y{7Z3^fc$q-oH&>ICG`?89fAft;_;goo82S;nWu6%#>aT ztM0}pV%6R;03hYBN(=3IXB8RH{@fA2E+;_F!N3SaDy0|p!_FUhBx60~@ zH%TtV#WNH8DBl-$I~T}8>DIsMV-its137wZT?A<+8H=4IXGGw+&qh(H+vJL+xRzO;l;VwKQ97J<0Sy8-5>|px|kc;5jsYOm=?^qv|1Tnn^m!I9uA@s3s z55aU~kZaiIo}ALQr%4z+I6Q`eZ%Od9|c zVQ3wxo&%f%$o^M?1Zp5Sa3dqOJo1&D$GWK%z;sg?3)Q>(WvIN45y#xHo`ggPh2E~u zg!YD6!%kQja%%l4z|^J1SabDLX3A3~!UIExqPfAVUVu^@pKMh|RkxkqQ!C~G<=kd5 z4Z-JuQQC~0HGFku4OM-gW%}Up(%|soP&wEJ!&=XxKK@*uCDn7y!>a=Z9(?L=7P~Z0 zC|fuqJoo#s;P3s}hF6m81Puo4DL?8zyL|AL1Mwwo?j9Xguq|V1>8IxQx@GKXH5a;0 zwPVfzR`>d^EvD9PKhWd6^U*m*EBW z=4DgB;(%(2wiEotR*k*G+Rr|mB!ngo%kg1oM7=mi8T6N6T5gQ9&4EF&!?6QD`y?Zeq{69BP{%yQcr@CS8q3XiFotP@%_a(&BchD3dh(p4Y9e#E!9Gs zx#Z!&*JuzbH79k#iLe$h_cI)f=1u&VnUI>9#=>6wq=Q570b1abQauiG@Q%HD$fAe? zJE+FPJDAA5)wRSa+doN|d}kD}6f#S_>pw}yYOf|TU%wG73|Lq3E1CUOX=GU%BEsSB zt$P;XhN*U%Gho7@Dh7>VwaYGc_)~o)j&~%dl%|%fVYfnW#&`#Y-FT6gpt?qRM z%+mU|4^fHRoD#)+^y0*2Dz2D_;Y%KBzc$Ri7M`x+?(zFeLEqFltoP$4Y=GhJ%Isgn zpfh9k=9-$*p68o6khMLD;ux2;RP=naMNxDmztGW88})drjLC$+?nf2>SKk5!yErd@ z?OC=TD?{{FU2$|Em+o+XHf3e*)e_nFfnzxe8`9DVeZa;v9Mi_PPLUhUZ{^8e_nc^1~pxzRMor_P$k!)R7LACv8QO*_Bwe6= z0cJ0e+)>9LgL{!$({W(EjjHWZt$M{|kkT~1)Os=)5YSVw-Dsf~hgfAlQQ1g+jh!~S zKm;_?Ln|3|wWvBgKJ|*wCG=;V*{?|PJ9sA2NhXoVG|BSS%_YprRllOKIcGBksffN| zr9NPQ*h}pj1}~*Uq1*g4Ka)q^uUL0oQ-e0Y{KOd@9g^909rITNlXC7n_s2>felM*) z2mRC~v7_Y%!@lJdP)V!mfXT65RL-B*^W7{BmKgMAO|VBfi^1y@kR0Fi-1isYB6pe! z?4&dtzPwCZref>2X;J3^*;z|l0n|IH&?f^&KM5?m7%o~7wcImpZfs+M4@S2^r8OmZ%&GVfc|zDiFUE|OF_h^IT2qm_R0yFT+n)x8eMhU*~7*O z2#OhVCQ*@L!7^@`VT@>q(vs!YO)8Z5y~(K*x&hB73J@g%0}km}o)_xOLTQ&Zx)jA$ z0w&pAmkyKG2^7%2W~aS?bIl&zs1Y>8+PO%AycM!>mDAOe8(R{V{s)Yw0K~H9n*OEM z<0#>ufCc5IUaN4<`g)y`Hy9IU+kOYe?venx1l}g(bQEpQYFDIQRx0%gQHjL#2rh=f zes;+%oD70hMbX)MNF8zD8|FcDgxtWo*?roQ^Ju4;N3~lc6ImV61yl8#-7>J1J!?Vs zJ>xhc+`fG}G2z`^seA0;H3cwE5%OLwzTwH+PL2z_rZmcV-|$KGtyCx)^u;F|pxh2B?P0csSf{JgYhz{DR(0*Wd2&&ElY0EXDwKuuKZT&hjOd zoAH>-<{t|CjyZE_MKy$?nF7ky_^V9VCulPrsU+-U(63uHB!E4t2 zFax7qkUyF44B13SIv+MH!R*d+6U_JZNp&O!qB{9btX3L1S_kBGYjm)J^?23niWw!k z*u~YijF*_Q0L_|qy)F&u<|i(TAm=%rW!RUr&h?rp0|#>0Z(?^YgOa^m<4@^LA8Nc^ zYk-9R!PHwR_nR~4Z0?I6g9+7`s*enBQ~3xNeAbo7)l-@{s*IcpY5(}$-O2n z+i{{d=Xt8^w|`_S_Jyh(%Xfyb;kT4#loaJ+@pW+-PwyrYBtQ;*9kxX;Rff&sUUJpY zWG!XSsl%@)G<2+H2?n&x&tJYpN9=3z*n&HiHL5-t%WpNJ7;J6SloFWVzhQd^o~>c$ zLr4z$?00~XOfmLTbdI;OdKzyrURi{&7u=e3XbIIwnrCGqvx75JuJrBseQz9K~z$gvyP{N zt55f*>~)FFyOu+0hpo2xsJYr&mqZ}P$#fWh31&+pOv5rgd44lb}xOb$Qi| zO1*G%SJ71%Z8ZbY{wtiera1kxfq`ZSKze=(cIGG47Tm- z1+G(Cj8*DX*6QIy@>;EFuoeoZrF4Xa+!yVt>BBarRF|uc)R~}HZki_+ zNgX2`+EsJZd9oi>>VWmDfb-m6Sxz%$8;Nnbg1`8{th2a?&(}#Qf=i-t)aM$9moH*V#B8)#fT$(ng4V}c zsS1b22Gm!!!9*O@EQp&45%p|^)sh**hsG*IhR&WacTHTju~;qWVY zJ)nj)y}?Upoq)yV5H5|CZ-T3@Np3RuGK0p+p5ANRR|iJq+%Wv?aB>aYPBHTe@xC?@ zny+utoS#S*3=tlHFHM5Mxv=ymQ^xK^0(qy%Zik_|y^JTLgCZ37_WL?kCH2Itd zrLu8i7(>=Wd+(M)6VDd`V;LzUCp{SeiUB^#yS0fosl@IXVat!J{Mc@c?|;WHz2>tm zI?$QJ&~xmND5e9!1+Gi=heqU!ut3wIMVvRM}$IE>;d-t*{|QB`$1A^c;hM4ux+~ z5~h^K)Ok@YXg7Ycl7p@h#HVk2CwV<^*$KLaK$YfvTXr#@c|VrFuwz$5cAJ+&Hy0)j z8Z#gtr*4$uRv<*E3~tpO656A={LGdn8o%IbO!7iQFrC{zT{~IgNwxU9At{uj+(_jL z6v`f5#V(wc%Qx@}u$+V67fL6tLuFf?xR?lgCxePPqG?j}V_N6qWgssbsEcPZE5p?+ z@w$sdI+3xjR6nZsk_s&qrKi5_q1&YKQ}feqx5d#0fjC|E>LuPCfg%fc6kkZbd>B+0Up^${Y>$wf ztQG)GAABk;tRprbH#lPw!gC_L{sxyI*z?VnU7MMqHIbPB(Ma*ZO zN7jh)W46z~09H?uM5A)&*-VWQk`RPzfl*v0k+2SXQ(57Ql~E+TGVRnF(P(@6n7%R1 z#mP~n-*Bf*QEWs_P+p82%aDBKO)&P^LeP_D5?%*-2s}~le%oV?;_o(1uvWw3M~Q1n zxTi%|cS^iCqwn}y>=`>=?Rc1vaJw3kQu{p3F~$WMRR>qGkQ#FKJGMOXn9UI{V)>smjd97c@%SG+_e;vfnt)h zNC}?a$(gKJg%&rwfW0a1FyXTPG>t%5%VhDJBebt|fJF08*;u5Mq9z-XhREh<@7|-6 zxenPAM}7TR0f%nF;@A_ll<8gEvL#QxBJ_FYJ$rjtr=h&~VJDM!wa z89PCPT4qF2fh7vff;*b1w{{wr^7@zM_VSu<^{GoGXkcpVizT0o5eRouyAf~+Fp7A+ zYyn$kO~PHVij=KtWc9=B$)tZ_XYrD(Y{ahFHG`pmR0pm`l$r?20Ck3$p`z%hgwl^; zgtv9xpQM>26q6-{qz`qBwDCWEg~cw>CL7-FCnfX8PxV~ zy*0}xaG249NR&`d{mmYKyZ}A%n~bEvaD+qc-6@r+VJ(blbhgx)Zm#uA@dn!kev~C5 z=87Kd{32`Q;Qg()cPo5s3ftDYVb{7iF5jB#@PuI5>L%vqSgv#n3(1vvM4;N5aVyVQ zTB;MtalY5th}dBIF@k?6dw^l)O_dVt^_X8N&e~_RY_H8bx*ebp(9_ zWb;G6;Oe~htoa(>$;ijqv29fdtE-vo-iFbc^<~0VB89ERD3yJgQXJwAgMU?r5k)lY zL!m%d;2tL|lK~$CNA*U#^5fNXzZ# zvVGvUUJ^$PV*D9en1tC0Rs#MhI5sYLYqo$nIu5lZu;v~7Ikz2U|&CNK(7ldqL= zUf9Fsj6u2TRC7ru`NsvVZ~>Wuh46HQ3yca5J%K0``exzHwS#aQ;Ohd%P2-Y%tT zBL!5kGlUU6eKs{i@GDQnT_7^h)&-_qD~?QW$d>&~gH3>1=>RTA+A_4pX1*_cI33k9 zBb6@jB;YAtH3sg`>`xfQ3Up4)f!uk^GSg$MDBuaqvwCg4NHbdd9?>ezj|*IR$-_Lb z&j>(h?y#7JFp)ol1oae+htO-`Yi3_^wIoMw?$t{B^9kwB3DcrwXHQ}Whe!JF;!HH8 zR}?ns4|tZXHBH#}%%Y(R*cr?o9h?CU4rJJ4A=68;lT{L-Mu~wdQd? z&y$&S{j^#o7fCaibv_%&;)~V0lF;;ZB$iszXk$ zPw-R9(E@1vk>h%zT4Q^t)b!vdo;hZCD>Fd8D1;CK%1*LHcEb}8sKifu{%I>bm$Bjz z&gT_Xc}RTF)Cv)#(i>6)fw zx9!jqap;{$1OV~Y(|DQ&WYmYT(nnYn^=}>?yFdE=>ro5muKbtg&w+r{h$JO`o z;}5__RPZh8<4bWYN=y^`FpT{p9PTJ2!y71cJG1a8 zfSu28L93Nx+nrfZ#4UlVp{az=Vm;l%2w%1OfJDM<1k`=e@bjgJ#1orqj#f>O^U6&q z?Qrd36cq?C*W05%AGv^7vKnBft;7HZS_sm6#phq?PO(0c83i(0?Dc}!byO;B&FK8{ z9XyIzIyb4W8SEhO>Ge%in_>fq_nJL>Zp)r=9mnbu8VB$PYYlxM#qD1OZ}zdJrgm+U zI5xNDTYdS}=qtC86ndO>kGP>=4yYV2c=#7~VH!6cuC5>b8!2Mxd%5jY`>-XHA_ZO@ zMW^EwJ46nhbU_aldzzI+6<_R|jX>h(-0a0bFo>y&MgFttx1P=bNS3djAoH;1C#Op4 zAw+8kg`nDz1b;svtnc@_(BL*bmMoJ)Y@RR$Cl8uS8Cq@bQSXXh_S3z=dt39uzMb`b za`|J_aDDF>aZr=01Y7gBC+A-3Q^!Utn4=Z;Fx1rD)+0Id8|mh8?%P>RzGx1LNgxB( z8#1}%(6hw!zq3s!q`rj%XTPIE2vMFn@pmMX&Hjf$-)}2=^hEi$x#z6kxo0!7(^&n# zz$N2^u(xm9hi3{9ngrW|tfpq~d8dY}WW^Vco&KabjNH8uj;mXpN=mB+!^e;)9UwK1 zFm9pnG-l3w5J+qB*l08(d>XAm!j+iJE%UQ292-ZjY-WT3moyuTQlc>rb3@hE@!$(4 zmf%u4QjPD$D8oQE4x)Z9m3M=^T5S0|jDc;@AOARhi7LOW*i~UPTuAI1r|@7F)8T4V zr6tI~#A=V14|wUu>-`%1)Dlk_T{??2;`6V-Gt8Xwe`lOcuAgr|GUZ6chnZ zg5bl506(QW7Fi5xae`G49(=kq*IfCq%zHWru4NMvyBxC*9+3p;$dGzETvLD%WC$2y zex-_%SepF=-^swV>>zy0T$yp{eE! zI*#{kf5PV{hLWAB)U-L+CA)A30gX^;zaXy#2Wda#;2^|zv4q`PviDS7T+*ZUz3P$- z(T?QZ6mYv$ePyX4e6}TB%o|`+Un!yA`=9?(uS^{*&3?E~S$;y6u;H7fw2_JG`?mcZ zy@(S)sR`_HmTc?S*TK%`op4&1ZrK)OrkXGsZg|)IJ7>KwJhm24g~O%9r<9@A)0UpHlZlELMeYhu>Qwc8rP8RrwF;%fNx|40!Jck3)p+K|gbA?^ z!LMrS5N_YfJW$$jP^dy~u`j`O8?%ZZ>mi5&OoO^5M|uEo!#~-;$zavwiO2g(Vn5sZ zBN3g8P>06i4VwVx2HRjeKQ1KGPFNPwkK0%+8$6lD}d7RP!eoA zJG$E4u~C!^B_)!^eX6DJpv%xNMJWiJ5`OYx@ufuU5d<3}_szAjgoC^|q)dCi4wJf< zwdau6XG@Kc$FS}vKJ~e>_{EL_04KFAK17Sxw6_@Zqlf6egRDy{C;QIo1mr?!&Fjyp z%v2~iO!vT|p&O0;9W>8XTtWc{zr@*pVxu>E%fQ63K{9Qs88wM1gz(Yt285W1+WEkc zfmYkIDeAJ}B`?C5xD({{Z)o~d$Am&|GLHo#^ny}43S5%^YqnMxE~RX=avfRFztfTA zVsh<&{T6wInsf`b*10aJMIyAA>4h(vH98vJWR*Px)}=e+pgTbt9k-|9%k9>{N+jmC z7E8KHsF8w(M>dfio z=p#ab1UKCyuETq;MdV7WvTxz)Ncn$8YA=@NjKl+QqXpD3iS2c;1&{t`KDLhBHITqs zfx`KNPK}#qglJZmh!!ToorBV^i}}Gp`Kc2}i7MpZpqw-%V&&HO+T1D9YBzLbFfo}& z`3j{VtLoxYkL(Le@e+Y8r!#uWlj}sEy2{XmhWQ;sr3=;cDrfJ1QghCEC2U=vDngK! zVu|EBW?~i*;Ua;wwh=PCr!mCsaB0EtI8jeJ>!RW+nx4ewi0#f%B`+OX&_Oqh_n_^l zC=f)wl)t^WJtFyZ;P>`722eEX0Ru=h$%shbOa4k4;L21To?3LY)g#OunMq(#Lv!RR z!Mvj=d(@zqL|eyxIJ9^96&HQy2n$p)Y^ckYvo%M0L#XvNu7*x|@sr<>irhh;?kMH`u1UL|v-vtyYo>^^1U{Ev~0zxVxfEuUZ}m zHQ0Zvyy1V;>c43_u;hck8rO#+8gbhZrO4Q`f57taphXYQ&0^8~O5_nIyBlC#l@o8R zxx6T*a1=aw%{i7d4NrTa_5)j-TU)&E1zw9oXY+^PgGS4Jh+kQ%RR@GVrK9c53!rG> z4BE?wck|$8kMgXL^B6<5T8}LnFWz3>mv)YX%Sh=KDnK>uAXxFrl_~ zzO!4)d*J0Ol>X-B2Vg{k6}~%5Bl+C3f)EY<$qGtLm&t~4#7p-e zMLUNjpCQ9(#$BDNFhn!Xi*ck);G9Ip`4yMNt?^lLlCxg z5x1~0gW%eZ#84YFk?2_a%^3xH!zK%4_q2@j3H8q}e9KH0eJo3p=%*GDMwOI+W)r8t zeBm(lkiwUFPnDJ+B#kF~4Lp-PhTS7X^WRJhXgh1--!mlCnt1T0chK?vwit{_C*0_O z^33ceENX0&_*&%du1O5;kz&A&|KRqazlr)k@?`#!vDt>Gov;X1_IqE0#yyK)IjmU5 zXG%m$1!0sHJYxmphr=AEqQ_S-$AwYJ{Y;ZLzX#eLOK8+d9==u|x~!5B&|XcF%g#uE<$J!eCJgkMh=u%$R!R4jzzcqu1G&hq(zr|n!$|g-)IK-K8I%?o3&jN zKNpkdzXioYWrs34PVG3A-l&_DNRH@l#@wBUKv)p028v$i9A2)q-1leim5IBVjRPD9 z6^A321wr7ya`;k?4>^1c6Z^*o#V3p@;*xu;MrMYmMyNgY<(w~#d`DO;i12Nx5X#%A z#`UGO;kQPhV9;sGhcPeXj%#)jOqEWOpzZwhr~M<>ykUo(ZNljqhmIUZ`Gk^PLJVQ> zMz{+_y7{&l7iP67`->kk;nAJ0`@^KO`le#YRLZO+%sjMSl!l*SI|a^T*gSajo=5cM zyrMdBx9m9O16%OcMK|?N?l0}jis`MjI{uS?6s~ehJtVR1W3E6sKz7BjDw6Aujqfgv z5JPK)D)_&th94r3TvU4hMG|N^_L^=m-JNRLG3rGic5jX0gn==*D)xq`T>Zi(|)H3QxkYVy_?0li3%v#l<2TN4e zZNvLSwZc{0a|nWAW8T`s7b%}5<8i)`KL03TxuKKc)C7m)UlLN-=`0q>I`KQAX7&*< zuI{J`jq0jkS6e(c%B&2-J1ZL}9jgi?9{! zKqrD?9nLXyCj3bwxgCaVoIUYJquly;pK^|>zu_kIb{^v+mS zjiqj7aiW_-c7%7#Bz;KCIfR_#q`eeQHNhVyJdnqLdcF2*RwABva{nSQ*&(n&rp27N zIHbN4Vu4_vxCawNn+pD&Sl%w#xKFvhO;2Q5nb91ZuDpzY(@oi1<096#;`Ak8gYsuy zlrFk(+lj1#^eq#$-K)ml%5fJVN zAJDNFj^k1YZK+%GI$6lWJS#uxHeU3xvgtoqR!N6mh47lboHV2`U<$d75c|PKZ!Jx} zm0i(d!lBpy!$ALvSjTGQl)#&)m`0|z4oPQH3ar}DI3W3iDoC#8f?t&Tm5fPHavfpY z^|5_{#QDgUorY7Y)?8~)6fs?*ZNiDH%2ESXge-g=z^d*Kao|J415Hy1ll4XAnt=w{z{!&Vc?*2jCcL?e;-$T9bv# z=QZKYx`6vPN_^ky>OTNc7XUe+RKkCW+QOZ^E1;$6T0=4aoqihw2D2vA7Z;D608#+> z*qr>=aRodS1ZbgC4KI{x_E0E*QVDb6EARVFWJ+sS~O0 zagysuZP8URJRH^Tjp`+@t?|JA?QqZB^3qnK3eg>Xs=>oy_8AD;l!a!P+aJfY6rI_7 zB;%;Ygn54#&rlP0XaWOyMg1By7QnMRnxyHqX22~?@{*)*MX92b#9@#BzA}H|B_;S! znX@%2N=xox8DKQ*?T5UYpA9UEjbQd-7(~AC$(HGtNjb~X#B(zTjfhO@W4CZhM8{@x zxZK#EdDbv|1Kw)Tqt;6`&=)&~H~eD-i4!5Zm71O^dH~Fp<-YT8&J`BbA{5r$9fIty zU-vZ2ErWB+lsxhj*ke3%I%gej-J`VKV;Fjx=#B{b}04dxVrr zT(waUXw}hu%wO8p7GnJ@ZZAZ&O(du`Yj2F24)L948tDfCa^=Q#Wxvk@D6%X=k@mVQ;V~5Ktp44QfD)9 zs{$-;^x@+M#`qts*x|l+1^v5E#E=ezkkH>C4v5`KMv2)=tZb^)M;Ksj3isSF_cpkF z6iRDzg*i99exX_Y2c$2&?GidTcV(}f>AKCLhyHp3%+#LwN$l*W;s?DaO|j@RW+F7_aSGCvOt z+G9wpIyyGHHu*^FKPGpRqCF5W8C+t?oJ`f7ZFgAGOcygU7oKpk)#O3lTWRhttDN3e zC^8x_{Fur%#7@im!Bq}>@lb_-m`#4bYW|3f&Mm4U0lyFap}+nI)jby$JK<%?vcrXp zqJ#PTfvS}w^gz{u8RU!)qvgX~iS!lK8l+psI;iR$kHml}#-rbZB0%G$!B^3?VC!PHPdKv72O<$eT z`W(}y#^w40Xn)+DLa>w|yVgyRX_zNm^$Ub6G>H5Pek(I4m$=)X?6O1vsG6lI9a*S- zD~zVp2~Nl&is<=^;UM0^5UH>na`4|yqph;Dzxuwp)5-GPo_on{=`1%k>#oG-vcOaw zTwJdPZ$;a-KEeGD-msYS5$da(x*aFt4byeT!#yE*b97_yTxoBB3ATDz~|_cokdC_Yjeh|^!) zC^$W(trLyypa;tI>Ox*G?BG2gGBr~aB7MT$+48S47}7Mrbo@MnzS&PayQ(9mT~NTo ztcsO3V6=dnhTx}B_C)4II`?xy;7}8w7j+4uQNsK2P_zCo0JJvMmEqoqbouoS0Q326 z6`ofvBM;F|Z_ml-Z91au@+Op9*Jk28Ja%NzcOkH$Bm4fZvQ@pM1|W&NFr7VU;@2+l6Q`Gx~X=w7^rr@59b@~TDmjFrmG79*@ z02%z=K^|T07Bu>i9~lA~-SH&&@!y@?@2|z?XEcl#ulh#p{~u#t9TipnxQzjl(j_5C zNOyNiHwe;*ba#VvgMf5{G*U{JbazWh4&9yarQGuSJMaFnd-k|452&sDFdj_wUXA!l7oif%{uQwJof)L4A~} zaGO&C6=riHhc&@h)^gfQ>Kbx*%Tw|_KmixqaCl|2a!3?wzRoR&oYSQI0R?nBo=hfA%(WZvM`kv)c;sbiHbXFJ)A`ri(+lj0 z`&4x)#|3L|80zx=ba6Fh_Cf<1XTz(-Y*ubg-t3j;H_Y&6a?#+p)fyeIdhfs-fqYW2 zUAl&Fj5g0_4Zd*Bi1+1aM1KUUlc;I#G z^g`NM(AQZuYNcjTRnUjrQ06;VW{O)Fe?lIO_R$RWSL7|25-}%uiAlXQFyr5+sRb3)aVe-Rz`s(Z-0u_5sQuh)L=k%nit%&Llr?xw1Z)z@QT0e-iX@NdADusbaR5fPo#{S~?5`s>CG zGYU?B3DY->ECqi4oat}WXz*=6x>vm#QP$Z^PEVAYHb1RJFaPiAQ|@m!%e{M?`MW1;3y#~V*VWCUWQMd=vEcUQz^Aoe^q9`9W{`9i z^uMC+e$g5BlfTsgAjo^VG1Yqiq!~_d@w|m^RF?mL?c~nb5BP0f{4C)HzaedeH=YCd zjmA$h7!$A(X#X@vyldD(Z=ouEDA?j%yvvw>)Pe)Rn*L|&{M8U#Vo6yR7H2Msr!;r3 z4h(_!d*4my_V%wQKQvWYU#>myrnB}xDDSNhJ}4hvdmX8Cv;JmI&r(Qk7}0KGE-JZ4 zD3BD<9HI)3{|4SZ&?n`pHG@AlGMCtDx~mKV>+b{pC|5tfW>8DXE9#7|PgcfK0Bvm1 zEMpc2(=LpKvO7p0#3q8%Mc&9csk_T-ilIT@p*h8&>O`vAkosl}sR63+$G-Fx#K%jw z4N$k>|+y(P@oeXMwq>f3BT2V_k*c-<0OcC#j0<$i5(J5*Br++6;C$kd;l z_Sofz60NRd?zjaf?jg&1(|P*Rh~t;V!=@ z=l+IkvzWO875+3FX=(f2+&ubL>6w!HGsAYjGd=|PdSAXoHE`o z3MgoFf{G_y>pd^~M4c3JiM569pGbZI(*Xs`HklBm>NlY09prt`_DOhY*q-nA|WO%!a6eYGKU&Q~R)Ao1Fnag$j&|{#}Rp zg^Ob>LX|EbZWig#wDkAZ0yg>#Fvgd9{*CUR4=&13Q_C)pgh1B32mIMcQRU#FpZNV>7N^M=UlWNS7tH|uH zL`OK&A3aO6Ij=SKg}+Gg?WrQW#NPbrQ2ll z^}+7*7&({iVco6*iKA?t_`CAWo`bvcO)d8?^Vo8V=y!Mi1@!T=Bi%Hd7O1S`ZdI3G zy?KTWA;F1oL}Q_5#j(U+L8vpJ1g;s7KGd4?r+MNsJGO@fli&?mC>UDch5!lO?3i>f zsJc9h|J~kcGla6OgCrl!k0c4IH9&Xhe72{CUIw7pnud zGr&*QTYWWST6UeC{`f!T=tgoA}@}A1w>m`xt~ zTOh=xmp37TAdltx!x8*(ZZghvm7yyS@URyt>qX|OfmiCA+V~9j0TMtN{9(cD42{-e zYw{3ZG9f*#t*`UMX@{}?L@#;$VC&iCgGxGw@FzNF#^sLA2|oWfofF2DDEC;^J-54m z1e6RZ>1*s15rJ#iCcoC{mt%DAP`n5w|8_m4KV!8>l1X)@7h>er^>toxdL>N}a@F$Q!Bl2fxlF{=8ob1oH`WJOn z|3mOhqe9i4njjul|AdQN*RFjUl1^P|*_L3$O2i=~;YN8C3N+LF%iMR|{x5SMN=Wek z*L2zk3zSSb8XK)2!Eh2{W5^2GbDfXnS~03OkJ-ns^S>)*7?%7GO<1e?ZpUxtSxb}h zZ;s{>>fbq<*|!fIjX0cPEegGuAXj_`^q1d_PyG)6U3GM%8Z{&g`gm^c-_Nx^F{{P}*cxLx-X+c3N z;?K#(f8i-<4_`YrgsxU*l$xlT8mT!@@{zPf;CBmWn!(*V>oyAtJD$IDzjfoeL)@v7 zBM+74YwE>%6dj9cLhIg}ukyN@uinFuO&F1M5@oO+HujcOQ+z@*dqJ;z^=Yh;^e3jK z$U)N$?vZ}RNBLVsTm{5Rs69ZeHGng=4#}Qog{(xuB0*?Wz z!?DA0{fU<-y(}jOM<(m3!xm+fV8S}ES=MVp_KpZHM{}9H>c(it8{`g5E3-XOiI#r0 z6|I;6VX*8h*iwiGiNrG%l@^W?nM00sT)D- z{0ohPCz!x2)q@~c722qL+gB+cBW^8@ljNp)*A(7Pif=XliUhf*w!E|Opsv1r$ zo4TfR@D%hOO6Z9H5$;ZZw_y%CDEvdfQ>DS`x91M5IBv>}Xt_oJt{LxUR#W4h&X<3Z z@cJKc*xcpY+XDs+^OIsix-WMdYe}PX9lLEgxDCg6+G{0xY(`MlYe>|@@s;H{leLl` z+Q;PYfaQ~a`PBUO#Dq)qWPvhz*piPVF0O+h$ZnOx2rz z0kNd7C>A!sbb&MWr-H%e*X^U7ZL!=KtI{kN5fj{;5wOk$oHVl)$H4Z?-_RCQCWsyO zu!z7e7d#6$az^|+vRz@rV#aH$^{31Ak(~Z1Jo%cLZga7Y^EsNc4a+4SNY4#>tP@11 z8A>EkSy;B1idG`gghpnMDVgeEr~Ju2)Xye%Z;xrXmNb({pZD>g!`j(6_~O*?+C z`xW2c=@`jF^1G?LU+G@P$@g^3=Nf>H`RwP3k|Cn2vFoz+D|Udv7;u~P7!90l*Iat) zI`|YbB{*~n7P{?*v7w;eU7Gz(es^lxg7{g?otoywiwP*HwP3hX)(xv*cr^&JI zX{f|lT>Q>s#w7T+SoRBtZ}ekmA_~Xs>coq-Y;=PIWXuPb_^y0@ZAO-d=BJ6_N)o<~ zP#vq^I^?2!O>Q}LeuB_-Yg;3an88-p_Brfr!}cl=iX(-yzaOxGeetZZeeLLAZ?Kkb+65O^bJ}m4TVsS29N=<8WL8f!hzlPZ zSDl8qZ;AhiB4SbSzG}~q-O-9>$BOhF%g^1h_wuD}HgUh|A>^3R0}`7g|0cI(I^5zj zhFn6LDvOR} zeJwksUC4BV2J(mm`EXz8L2{%NR`0UTFFvJRr9%}P*@+T^X~B|Q<*%G%lrdB7hqK++ zahgx(Ue)kcBgw1Bq2Hc#dx#}{dWGM+s?oNDe?T$nfq$u!&w`F^HM?sMiepin<-vOE zrRQ~h%|IIdS^rVBsI=AAoFC8iJ9iOwtBLHrW^DHtDA)rb|H)POg}nW9u7WAjr`g{s zE>41woMn8lrH%Ogd8OfSY_(SS5581P2lfSQC9Ncpwyk0rr(RvP;*4dgNR{u7Xd{K5 zRG)`WGjofn0P!@aqoYr{E%>zC!RTe_TeXm}*y?gfMx1@EIx}=+e=E_KowHO`Z{9!3 zIt!{UjXcOv_=hC->>Vu=KBzTh(LU$&D0c<<^&a%+w}KFr<#O91yO%1cTir4 ziX;r1s&D_$ON`~b?N`L`#!?`M)xIM-7KcGJv~&tzAQ4qSY8)0mtN=kf{b zZneMK4yFcwGEtS67Su%P35H~)6wK(Z1Zy^{E&)IYM}Bs4|I55H^d|Bp>zdpuS@$DU z5Og}L+L+mTESVRsS(XvZ8P%Gy+EQ#KY=acd!($%{ApkoScRrXmyr=h&20g5?tha7c z_eyL~F{Ix@871!q2@|7|mU7j=DA5 z*yaKwl?yUC$cleK-laxyd!YU(5;7zgv>A$Z3Pdn;I|0GFH^AWVD^ko_+53OhJVpZc zQ@|ldF5+tlr=n0x5BPe{{jE4}OQ*fowl~$$x2y2FZ7eLE(~emhLLZY^NG4<0g$AzC zm$|OjXd;qZBu0>q=!QNH?yS2?6fBqiZZ*V6w@5lCW991Dc+P40K0V&IJg4h%=yaiD zpq2dJS5aKWf%DT};IWfg ztB_WtuX-J_U-%Lx^g0cZ<>#*PI&L&4naBc*&t_hC+zls&jFreJG<&Xg_|tjS`Rx|? z*`Rr%9IVwhI90MrV^mYC#g&;mrg4#`<(S#aB3h`|f^AXJV@K5z+eB}LBsDDN)n1<) z{G@nSj-7BbPHRT)INB#933Ix&tr+Y~kh_Jg*7J$QOTXm2to#DDWNTE?0ur&;cdtyW zg@x|WQ7fb0Qv$HiPU#wn7tO)nxdy?&D1ar`6+XIU=Z~q^FUf+m7KK??@9c zh?jW17O;Q<-1|XblkF_K@=`F4QEk-I{q7z5h!5^(h3zSvAoW4{!hZMH5zCX>V^_gD z-Wd$z%g5V}PT;@&$ou%=CxT%R!pd3{Z`uEvWrTeIu<=~<;x;ug~XrVhetVoa1pv{ zRV?Ut-v0l(S@q)kyIy7*9($&r6;h$5|N7G9ebE&8licj(JHg~vlW1FumBJAWl)A); z?PLb?+aY}g?Ob&;UZdxX>JN+b^Fqxb2#HAKM`KoOU=k*h^L^!F;6ktroY%*S)PY+P ztE&sZ_qH<+?P4&m!p1t-y?{dZ6X+HcY2x9dfoi1&tET3CU7 zD+02g9eDi-z4de&9dKoO`(`(DvW4HWCN9*-ke zVBkkm&ak+hdX8fdi{^x(_^9pHV4iC#Xt_G2VlqyhOiWJe9fewo@~ZbpKeq zd-*)T%NNj-9=yo}D-Rf*HN}%A<=)TIBS$BTVIk**?1rs}U{3tE|1* z(Tz8UduPw^I;*TsP5xI|Y*13E&^=laQh6A&OJotb?zY%8t5mr7d}Exq#cZj3h@&=s zlVZMD}Qq37e7wM5j}#0jE^>7 zbY;(FN2XGZl27~7(EZb_?mU`>UZ&I|kAs*ULEJ7oY?qEm5g!b!=Gt2; z#9fRZNjnoViYOB9)$_NnwgTmf=d=ne&gs`>giHfw2%+geXLb*l?n2kLzA=05roGLV zyvbKyb}Ii1P8_LMW)bbZr4~i=0lK?4(-HD_gOe*+SXDI_SZ&Ka`e6%gE~}ZJLSGf4 z7i^ZdDETDXoC)1}+Y`i=AD1Q|C8cN9qg`hiMXHM=&hjvh5|LU+jtNuzl25iMFOwbG zP-qw4KV(f@*$q`Utl&ZT4ebkrhloj&M(PHmp}XUiC%ri^UI!bygFOgd0_ zO5y$KCTktI&y!jB3ppodKdQco3%To6zm3;?4Lfv8`@?F>tLVQB_!1&bgwP3B>+DHv z<#TPbgN(>~EE2I>K4}VEBeA65*{3Xl#}Y1yrG3(RGijKOgzo^nQPVq|G79YGefa?K zAiH@(2mH3&BOCe9P!lUZL2u_>A+dy^tu#lIJwz||j=6G#UxYJ?1y1_XMN4+RV`I%K8OrsrX6don7{;>$Z+4>pjud>v6l?@J=i*WX~SkA5Y2#B^Qw-Jmm~ z2ZI)KB>U=g9F1gW}4n?*}M9U!PeGXALUE?D<)FsM4Wi(`qlnJ5VO1B|eH8jC5%})V z-k515pB>apuytMwDZ$84|tXiE|wt84>*x)0fT=mFlPkh8P- z+*K*-mpyQOgc^_|B^ER0nhVg|yD#XL8qu-~Yv*n0%Xgkd#nSn6exASR5>91dMyN44 zLt@k@+kVlJOJC0U=Id)0q9eMrP^(GcPF{O5p`E{iY)J2C+3_YB?hdLnhp8ll{(I4H zE)Pc3LNZ3oANTv}ihD*Ndaw0viDrvg)sTmwlrjvHZ7WK4H{`UUk2J88yFRam?o~-8 z86tIn`&y|^?mwe{?FaWFCbdfIOt34yR>&h6S16Gq@hj+WE8hLlSnZIpMgdP|TP_2BY z6@UGNojK4;#aOUnz!?JR$O;ZBy4PX+sk->1U7)tGowW z@%~vP*DO&jkeI;-z5eGuO?CNHECQKVMra0OV#OSQUcQV<> z;}#)P3$CzKT-!y-(qEbpfW6&=MZwTQ@-u))-*=5fE8wJ|ONrqosBmd~Z}z2@{%u9# zx=|U&{ngB}ALusCP@ZAsQTSF3xMmUCzpQ$N%4Bh_f1+zkpEEVnJa zon(w4?`27y==UDt;Znz1_xb_0GTKdFx+FS))dbm@cnf%F=2JMG>%ZfqZih zrE3h?Zn5+VrO@9{_X~+u)ej=c-B+uzi-WG4?x&4olo^Uwg6lvmAS`DLhI|q{nX$Ik z#xnlvI85sI3}1Q;7>!r@)NY@{Rz^xCI#FgXffkRAwbyo3#>&e?SGmN!uzRW)+4o<& z?ZD3fRBpM!;G3j;@MG+`&ueJ!E?oZS2q2!9secFHIK3WszfT%0C&mg)8X>bK6dm3) zl)gWmL;rqn8SiQHgR==1iRU-1hR3_+)!zVl$-U3?1$SJZq^XYl?#F=9d{{*W?yah) z6-I?j{``yQPv308WY(;Sj@H>4FO7CoVhRgKuCIGi+(mHNnvVLE#Sj##7mo$>rT5;6C~Y`|ToN?@Flwu6w6!`OaB6P9zS)=5zF{ zhvE~o3&s0cztvCovwos!H3xmzV^>isEm8(IBhPMT7LqUTAnhC?P>hoit1{)qWSuxk zb#+YE@dl$TwXpU=R4ZcV$3Q0f*icQk07&t@Ww7lKGMNb!EVq&1Lku%a^f<@&_v! z>#&+NSk?bf3B7x=awY^yJUl7YoQ+?VCFi|Kti&QVkX>x_x3Z)^!+^{Ci0x&m=rWBH z&d{PKMU;EiWR{ty}M2PX!AdK8*l)%#jT#plU;!4BIMz1as2A_19bblV`?Bh2T~#GceMxtq}F@8 zsz;HTvEs{$Ut&MC8FX4RqkNsn?G-2G>1lgMOhCN0p6}wrwvX0>Kzzun6!E3T$=jw|MIbC9O2(Th<`K{K4KkXpog3;VUEv&>XIU^f+URc zpZ3<3FX_v6%1L+_;oIyK%r9ZccmDQs0H$tG?!DJbGl9M`7XpN}8^v-kjvnCf_5_g6 z11YmV6sBeu)cLV0GBb&vWnriG227=Me=TETlwUgpr(@0imnl8T9f)HFs8&Q;LDNH~ z1~Xo^j(0SDLEpE*JgZOzJQ?bWt*VSn$K^=8Y$%5CVP%>f4=cB zkMsKt$7bgAAuuz0)O%2(fXK0dNfI3}E1Xl4OQ%AGckRy(hShOy)b_qj&TlZtXCS@)ngga)C7uRCzcZQOvH^&+Bo(WCC3P9#-tGH12 zAp<{Oru{yKf_K0Y#gQ*KV%SA6Y(d&CUxGaieoiJc5q1JZ0j!n>sRb>=?s1pxVEo=4 z@OqPbd}vDfFx^~^>#zTSueJ_*EE>vLR!%L+Wg@Pr&v?zWL=wbWjB&P|%IEVVcUzHT zc>VaaD3RgxNyCAtT>7Y0>IFIDnF$jNiq@IKT(g=!8a_3tBn+d_j}#F5Mb zD-hWEf}5w$`1xPCAcceCR)9{SNO;=&+vP89e%XZ3k_+3(C6Dk-$+=cLZ@a4?>!g{l zu6O>gS1iMsw>ItCa@j@yoenXn<6bu%jUZyW{M$pxH;Rl zL|zKh18x!xd5|fe)!oaKeoKiPb(@sj-Y@4U_{9{02+CAl-i_d^+L)3$n46wDe^bv@ zZ*!R2ecX^4l3ht?=4yA|c1N+&o z`$mIL?wd+2b9K=6bZ^x~5YF?~SJUJuAVw+ZypzkRy}h5V<*T()7la1UU_OrHVeBIp z2^+Fn)C?=K$sk{WjkS1(2AddCKi{{j2JiUPhJydk?E!FTsT zu#cg!shD>G8sL&{LO=kO{KZ5%uLN*OazP)H-ES`!m2-oE@i4z^afS?e(fJX?O#*at zFj9aLDL})sA!T%*L^B>Dc|NL@5LhlQXoD#_nrwnp$KjzQ;H&$Ot7(f*zdeuKU_`;OKI*t{vJ~!zv&6T55Ug$`AGHAp~ zatU?>?fL+sI$pB#SP&qR?FjX3TY6y(+nXwUZf)CG5Kjc>K{F zKy{1gr3~~S2;7#I@x&a5Y-)~4bZ7PvGN`ua<^`{6%Kha+E9b-bU?rb?IHp-4+YZVW zDOlGyu9`3}dDRAGnG-|2gm+*+q}m`^N&HP}I6V`v6_c(5&uBB|B!$X&b>yZvYT7B} zb&2TOgKNHt*=xg@8D(3Kj_`M3ns9{N+Fh{?ig$7Q!gH;=3J7LC;HJ4)RyWMxKwM|M zMz&-VZSsnBuC3e8@XEzp*mKG`;mH>*N3g*J23*tLqa;{Y79~aJ)$SdIDIAchRa#@a z*lG0Sp(ZzSJM84dqv?Vy*JI@P!-I zTo!?k($kVea`&?Gu|-mH036JBYxqNvpVf>rtC!&8Jl35%D4^ux-jURBIJMX0aGzPC zX|$zBMnIktzZUD{Aurun57gY?JjlW6g>tZ5S%-T14R%AAvoDX7#n_pBV*Pid%Lc55 z0vtT{c6D(i)S<5G-{Lp-z-Y-D_hmF*9P!Z`R_uN&ukPC*Ixl2SkE0LMIG}h-V+Aiom#~XdnG0X@xktMgWL35vw9hkipA{TIx^pc zHcgjf(61uPTtq>SsD%75UP(wg8STABM}Ot`_qF{gbV+3185VM|;X}cR6I_ z#xeGgomzh=tf(M^GakU{wwbOOwdgRWmDJWXB2&U;74+zoFSWkInty4Kb0Zcj`3nJg z0N9;M`Ix&RK%@lt&O5fZpPn+5DRjcJR9MMa#YCjH1;w)ig))Pr{}u7lPot|nKB3#R zfs-UxhTC$*QTY_vRpIh2y}O&5eIEnB=(&#p5aq+Ye5|lO{5pH-3w7%F(3rYwhV=M{ z;e&a{xaioOaXFYmw@H}^-k~jNTE~+P-2=z+^OdVjlzb%~4*P-_05}kLy$_BMK;u zRSl+f#@q@ah~B(K&u4dhuVLLA&Z@AI2_z*ySyS~)1UfV=&IDZN%7v12*SJ@LldO(c zXQ+3(l5-T)+}6_gucVl_WKvu9b>_*g=A&NC0VWF6Uai!~pUXnnH|4&FsM@_$uvO zAV<_T%USrOS~L=+rXrMKFDEwQ3O6~8fgTl>LQ)L>`Zh@JZ;wiY3PZ!juiiB@L{V`u zAx`!wr1wt2MVvf}`s*QyRk{acOu2o8Z(lKyoakXeUAbF<=-uJHkMH0fQ;d)N-Mj4t zASkI-E9QA|Iopy<5a1cBH-?>WW^ajCmH8oQq?eY09E1hzr1PWJ|%U`};mpUUtH9KM@ zXL3w6JX7qrumi8P6X2uM)@n_M{DN;N{(jQ&K;bY_Z*EI{D|-T8MHu=$;|$|@Z1$IA zk6ZVFyHk^OKx2rkEJ#mLTP++3WF6f6Sn?3l&WOBJJuwhVW|4;YH8(#F^D;fKgEOpb z!>X>gHS=oNx;v{}<-2Y_S|;a)rruYJ6t1NN=b0bD>XeiH-Nasq+gM> z*>&XwS;Wyz!wJ@t-1{u@uiOSgxf(9p#84qeP`_*o7ts)_jA-e<<`l$v*BJQD~ z`{2X3h+poR{?n@v@^QMa%KE3NZ?2JE`%Xl`B$y{!HdRRsd2C_IGkcc1v}K7iTA4PLzA6Y#zTr|@%WxArCsqneF~N>#Z4zZ;KOCt$b=x9S9c8KR)yfb|TKhRu|va{nBgSJKrF8VwhqaYSZ$I#kXP;O07mNlOk(iV z(T9#yBNi7}yh;{y$+UY;r1j%4y-X+K)qjVE$6k>J8~B2H{>|~w=j~p^(>?wvRs3*O5p@eu?J^93=t7or%NA)F#8Drx`^*6y^5vxPidj#_#XYW&h?FwIt< z!5&+1^;}5nOnzP2mGgW)va{Ipb~?v2K45a&m0b)|Bf1qf?=#lYwBqCIRj1vvq<$B^ zg{*}C1r_TuZ1~b+s;zmb|s5>~-DYgbX6&Z~4{(9cY@&r#muusRAII;{ z%zR$e+`;uuK|(oyeCqc~0A-Y}FfTZL!w-1>`>(NFvYync(#rY3?fQY>g3@6RWCIXD zW$ZaD(ROEP2OOrlPZkA$?z;ar6{&|Q67(>03MAC?>j~>Hl%1ucMP}0a&ma(X3vzM5 z>euYH*1lv?s}oXQ1g8V|kR8|8ZAcumTKa{KUj2Y08$#9wDgru!HmgH_g2jxb$ikbs zq17jVsN)CV&+L-UFgMEoO?XrT@p|4uQW?93I#GFBLN5H!p@hnVu9=ZTu`JCgF&&N$ zzqHP~FI)=6Cnl~HQDmTJZ$rJXi<(!mrJX&ZhqCYkQKeL*Fv-dBoM2XRGN zN6(4^b^Y!lR%xG~-=><`yl;-yAX;^=+>t1u>;>6RIUL&jGpx`cI5LZnU5He83mX{+ z&k8l9dDWhU6ZT>I;WmM}$2vmJAxOMe>vLwAk%e0M% zf>Wd(e|f&>=h69I57w(X$19HIzS}cqoHe?+buw;$E6rZw`^*TCkFSQJyOUnWfo81^ zT%FsCBGVG%3lOiNya5iz4V+9W^jG~0nvT^;)#r}I@vMUR4F9rtM{G4n{m0pr!^MTZ zyu~!gajML2=wwa7W*p{Zyw;GFaEuI6Gka6RQOiMR>3Tt}xGvM?iNe{i@%2B|P^%8C zA%6_iZP>jZb6OG1%P3%P35S$n44ifNVE4h^Z-kj^6tHyS7Rd}PIe6IR3H6+=LKUVU zV@-3^NT{LNZ@7*)i89=nbMD z&r&osp%|)#bq=AW+m~*X$QShNGQ)QjMN}w^2dl}NuE^?}a7D|T40b+_s&fCgf0F$W zS|VkFNWvutEQ4zMct^(S0+kuKcgLHa%ZJrTpUhtiN3<(UGM#`^Cp1AHi?BJTiy-jv zquPeVGMkr!-9{0TvgqzBa}ltNp(Qjh`s8}|_d;euao;`1=c9H#(fwTIKT=qeJPpJL z%XdodYNGCEaVq6{+;TkB9O-2^sKoC0sJGlVe3W3}CxCJq&v<=bM|Dy`liw%`a`#p~Jarm6$Byr-VH3*)! zR3KFA@MM9%a518wE30OW`>IY2GtH%UeEQA|o?*0f!Wai>{(O1P(o*TA=@8sZ7S9x8 zNNjrhbNqQR3wb-bUqf4#3?KnczC%3^;d#pHnEC4D*SO;&Qd_Fy-@C=urWpofum=PE z49BH=$N4Pk`1ul$pUZ4)zKN<@+_{jhIG~}&nvo$C|8`PaHKxC=%&kZpB3`SmMEbpO zb@OY)q;a)6WX21bFNt)P%I$V;^@fYg6>Gf%BSBOm*B#v>&$eoE(YnDN0;+Ft-~s_S zSN@UoKAYjp?|IW#HmJV43bWwQ>7YI{a6Ujl)a#4U8dcNv^`hT;4F-9IIYmw%8BN`Y zSI{WC>?7`hZu?B6=a&xtxU2oSkBRc==yl5u?oj=d|4HG>A`Yc zq(Av=^PE!a6i@_hxIyLVc5V5Y&L^Uez%aOOZA9B2u?UcJm-9k63z4}v3~r+jbc5e0 z!B#%I^K{TG%VIf}wB&HZJ8zTIqpWxG4ld$LxSb(1pFR^b$W?sb`VG1a@6)fA+1-Mn zXS#uEz~luPzuD4l7cM|g`jF!F-t(54pTOo-ZC9>i%kd9?ylDzO=4=an)Z$plZ3RdB z`S4O39^1bq0TL($dHRfBhPg=o4#wxB+F_K$l{i-{4klK zLt-PEoUTND%ulr!D9n3S^TSl!_$q&$YsukPW4u!=t>4;O*Lq+<<(|b@o%vu}rraXV zhapRi!!d*^*|UhcYB!0Jv1s%>VoKlQ29Cj$Pu11g5urxcl zmbv3pm}p?bkU&q*6SR8=k@#;?uG?MPjyRXZ9v97ess#z1>F_X@zP#=LN@c#gV-fV0e^IbTzb>6u-?Ra+;Id30uLN(>W< z(nZ<(Y~uTr-X)`T!Dm;&qX{3bUWcaC8bkM!f;+qA3He-*NZw1^v|PFPsGn8O#(kTq zVePOl@hM;aMM2`2eZbxK+>e3-E5U1mN>$6!mr1*=)DZkW0%mGS(8oBA@mcWzNR%Ib^Mk5$DQF7IaC!wDDH(BTp`b>R@ly0+^*s~AY0mszN+#t(cV9}T zXX5A9J#3?!cQTttctwg;p?Y71#sn3TIzPIkAzq5eZf*bFp*t#1u<`w*p_nCACWd&X zKB=HP8ZGIV`aXK{oj^Wgb%uLM-&R_`^mca)L0NP^aI&S)`re}niYu+{5P>h%Cet!G zLC99l;^L)z?E)1UT6eTZ?%hb7ey4eRflA*!42uZO4>U=ht89@d zs`9I>GnZ(^;`WfwCZS0qc@NQEHyDZ!Lc zujtP2qD%1Y;$~+AI<4DlX$bHU-dI2*BsYRX)~Ogcb8SiKgm!b2Fs_C3Gwrw!xpX2w zbdxht0u43%wiX4$BbL^hQ1uZkijXWc5(41ht%4_$--WVS${jpC zhWu~dc-n^akFn)TSE{c_1oQR;xm@)g`t*i>2+$yLvwXTz=vI5~n8+by9VoNUs?rsI z_B6EU{F|J;NAm|a);0SYD~W_WvlKY;M*7BUw>bH&kv|tR4@AaPp{(nO1H`WChfD8j zKblJOCZ8Ife0ZIYWjjhw98c4Yju)`ziX8q)zMa_HIJ@okA%K>zHMi^5lz{Yct&J(q zTIt)~=}4|tUU3BQC1Ns%>g5K_GerQo z=9#Mo-6M06DjeU06;Pub6Q)P5gA59g=(;J5Wb+axTdvvj+a532{?B&E`PnPCZZ9Va z;h4PRbaR5CC#c!G^Vn)Lz>1y!X^o0Y4GFCJDClj><5ER*Jo%#kgX|jkZF60MxwpCI zUC%nEOPOEq%7XLRfUmM6Q3Q3oCB*nCA|)dfWgf}z9eYCkjB|p5V+&|$v3^1(L64}i zvXv}}IQ=$vkdiR8uc`8eypgI=4TvRjaBH(6%A{&>#EgLP+^BRs9D~da^y5H>XnefO z^+k8eBgj#{MslOb(qD`Ia?Fmy#*`jh16?DhOxMSaX`q`ue#o3jNn(h29zScI#wg!7 zI_W#+;$v8PM}Q=?R{LAQ9}48ap|?Rv#nuo%FDeVj}OSNhfnO>4L~mE-prUYj)k%NDfa z=_k$W?{7@C&OiO7f&M&qRw&7#ZanL*nGHj)PQb}weHkMI{P@=|X7ahNL>Fs)}3#8nm&mJUQUj`&C0UQ+a&ELfc_2ANgR` z(%A!dWyH1Cf4YQAyt>}U<=ZO-K5$g8pZzb6s%Q)y;+;`U7Ry^bjS?Blo@%z9Q2d|HIK|kdeAy9PRDJt@ie(R4AB;28q2?4Hl^t zFkLVmYD>Rq<_J=EooC6>l%rZ6zgW2q%G(X9U}!1Iju6@1zL`snN71-Biz)5ij|h4@ zoQ2(iv+)DHV-ndVztomkp-N8GtZ^YXd4YTmHRs=Xq%EDj0kK;iDSG_Dt1=Ij5*H|9 zsy(thhMw7hvEYPGvTLNAis)U8umHEGo_Pcjc1uF=zp=ng?BE5*LZSW?ni*RsRD?tB z)n*($@jm$Gs-Cgu%@Y3)QG`{Ix5v3mf>;u0<(;VmhT+vt)N8OXHI!t>q~>SCB;nAw z=^VAeB5?upzp0%OAP7&7aqK#7eVU{ahTh(Z+?% zl)Xvl-de*MJtLVeZxSO9-}*0VBJZMWYJ)nwA4LbI)eBR-0?0c^nC5c?coXOTNaBOD z>WNzQXt|Za^|!eI=|iH>DHb{odlSVQn+o{$HtK*w_u#*z&&pXN1EzKjq(UBLsB~Kf zo}5FgPt6crFP8i~_5vK=1G7%}mzT!B5&S*oB7J;Ne_Q3I>$t!A#RhE|h(Fw%WzSZo zPe$I3lg~nuVOd9i?|fS8QA+(MEpkD2W;KE%VILKgP~7tg5x`+9(1l zNQX#CcT1Nvh=6pLba%Iuv=UMaknZko5b17^?(Y63<&MYw9v?rHLl-QVdChT;G0v&1 zO?cSTXxi!i32-*r7bqk3@v|WC_aAM+%!}A5qn($&wL=?~6-(&! zVNlnlBWyh$x^+?~*^7;O#QLz>?)Z1e3~x{C6l^j6$8i9H*Ve zg3Eq-Rt;J+%AsnR#HaFdIQ(5OX$;)_Akyw(*|8jR=z_t2c9y6|SFy_jLqyFHr9}Kv z%Cu@p71*z*HSWbU$I+dqPfUPNylH(Me_tGQ}f9&j&R!-ryB)D)bW#19Qry%NG0l|n|L z<8A0;`>j*peEfq>@frICfC$iD@wJrMmuZU|c^3cmt{)4%@01P9>Upm#33{ut0k3!| z^Wn1lwzIbP$0XY?Z|sEz;sGofXYq&VweZN@>`#7wu*=QHRCQnyE6*&ZzXLX^06y0cA0x>nEG>GaJ5d5MTKI^wZqh}fXGW(Wr<8ep zfuV=$yx>P%K<87RC&Zn_E zc0Z@T;J5~K45fXEcgnmWi}lWnT0&ry#&mK@1V>YKBWyHgenfPw45PFFPumh6o>gZ< zO4+Jj8%U)Zw_P%4a6KO$&h&{mc8of1QWeK>2ug8gWz!hx3W010stZhkAeOo~Tcjy* zfnP4)|A~^=1HJLlU}u(9y1dCa>{lH)QMY5vjr1~L&->d-Uobi61m9_dlUxVc5Kbqr zV{`_1D}44cA?B*)hWi*GE9a=8>4|cH+fAmJv7*5`wmJ@C9nGZqfzQU=S5%DcU5bo% zWgFv_d`o@Mz>f2Vrvt$otnYXk(Put)rm_q|>2o|TkBVZKi|64S3XY4B?F>x_W?I)< z3yhq*Hbmi>tcEAL{_OJ=b!IuKo6Qhh6jYr9UU$CBiNTXPZnG4aAao_K(@ig(L9=;XQ``74ks~!z z0R~;Xx)p_N^>L61A3q!VWmzoI-`!C)fP|+06p%-JQ~4hHzU1u6V`c|SDmF?dnGJa_ zLQ8T7ELyHr33r1*^^21CZKuL!Kher=+fKQAcFmHyG)t8!5m5nOUWg@ai#nZ*NPF5@ z&qbbW-Op;NQ!`(5&FqPSM!DBEyG?qwZ0@S%y+*_R_GB*qYsfzvu`!Tu| z1o2ZiriH=bGJo%`8~2lj=AHWq9O9quC)RMH{rs>0G(T-0MrKL=Qz)VGxNq!-AqMU-AdZQgUeZEYSn@W}a9Q)bnU(}Nut&`8pQ7+Z zJ&O?r+ARbp&l^7BJbtpd1{j+Lk}7MFqm1oj!~8a2~}WFmM|4s&(3nbHd9+lZ!jXyN6I3|I|^o{KU&Khx6Nr`zyLc z&vlf$lGdX0Qxu6=8yAr|x;3q6GocCoYc(TP&8_2x<)0>m%nBE0XAN-A&O0)L?RGc6 zMy|=|WIMaDUI)IBLlhnF`AGTFf(3Jg>QDG8;_{x%|B2*oniT%gp|p<;g+xw^7sQmf zwom@K)FJf`(TQDfidD6i$;Iy@?UsB@$~Pg7>It7;?wqYW@2hCkEx#&!#_)+$27op0vr*hXgT$PwSX@RYZ4H+DuKBiV*4dCH07r!B^`Rp9^43ol zd~HldNgeekFPAdI+A$=p1A7m}v$zix*c3Rk(M=w8OSC6!p^_mNy~u3``hWR>8B3=L z*!MRp7`wQsQ~^vhn}~{$W%S+dC`(aR8`u~n%SOXPvej=fJwCP$#~02*%uVmAf96D= zcPdAg*|v@mn4~g{5r??+s<}B1PDGQsW?zu0Qg;3*3NQ4vY$1{d1A~0@f7pSfo}Y#? zOwx&g;aeFur3P>wh!rVHtt zbOKfwh6cD84xe$UxU*tYFwTo^3k=v34aF6H+wY`%UsZgst&CEQFW2g3N=%tzvzM(r zq(HulFhu444vMHfll6-ZE}AC3a{vFZ5$(_*AXN>#NPp9%MK^u3AAiJUMRU4XtP$-6Z+N zRo9`wtg<(D0EM)CkQcFUatXM2I?#TzWQmu6E|;_@oJ^xC6b=`I-h@}4aVA5tYo!_G zms@2;Vf<1xCq>O0Ik=3Ud?37u&VPQ`YzG#n=QimIs(J(79idz2ki z^{=P3Zi!*UtvB6#PBTq|Z@OT3$~L-k1=26H5=BPx=r8loE+XTU6en62ONYZp_A-C& zBIlCo%b)6$B%xhU$^I=g14Mi`8{K;NO>j*x=PWf|Q@{hZt*{KvsZ|~%hZ29>(ozaq zqZ7DtHXu4p>Ma@}89Qt5(}Z(+LnXs(Ff~T^e)082-?Eeetfq;T0_hkSM3+)QA9VS* z`r>7kVtyiO`Rf3k2?QyX_%bm7VJzpRRbtZaR1rji8;` zw-V9w-iaz(k56w!mBh1W6JB+-$Z^JeKPb9UV$^{(E|?!k!;Nh>po17L-jW$*^3%BQ zdM-N>Lcia+c9|z;or63N7z~-Vn#eAo?hrFJn$n-{3fgOD~H8Js2-&{mm&w+y{>k>NL*@;|QSgt6O-zepYfwxHcT4$zvpuKQ7-2Evf zv90ZHb9Z=^e6hlmTx@hhSoTyAR!O~AUCshe{5}0%#F(V4xfIo0WtU5)U^9B^1lWvT zKOeeUGoj#sZO?lLh*HCZn9@m+oV7DJ=UJbLV+^Q>DgtF9c6^(w-Sz2OPo^Q>Wh0Wj zNd%(2)!fq^gVV3~aC521On;DcC(*MAG0Tw2zQ*}G1kLf)P!Bz06gtnb*uRvQOc;8w z_QON$qCmi-Q^A7-)be;ds%U00204!Jcc(%ZbOXX;d2CMzR+_J?#&1+aM!-FkkEqWu z#7#=`Os;$7h+I|l#UEwi4*8(wQq6{B(Fx9fHe>3*w#*$ zEXSHG|D^0K{PtWsAaBx6I22O@CY%4aF*tN~t;HX8MdiTPi&ZuY@j zd4O?C3Y{DXSK05W>dI6#vTpJGW*>~phy_cgU@|uA0L;tziX~Z4Nn!FbtsG$r)93ZS zQs|=DQlFPzybq;q_5Y>P_m>I-=(GQ)3N!Z-lfLxjfw@T)u+)df--~T! zR93y?|a%;BtFSv}q=Es?AdU zBOu6K24Yd$BcFV-2OrT2*u~S+aAiyy2Gv|O+(qfV8}WfY0T2^S&HLFGh&XfX;SxgF7>1TL1@7ll(xOHbyDYYB0#Qx)g}2gYd4% z$aj$*bsqAwOaK;#8)@c1QY?RO8VevQP=5wUeKY^2|FO$I$3+I*u`Pgv7TQlPcv8{ez?= zxetz(gdhLwXjwg4kYNm}Y8D_dO$w6DqoYOrIPOIsLV*_U)ja=&4h=HIJ#h6M%)2v* zWyRhBWL;k{I4|!PvKb%QygFfOm%Ab^?Xt4@zho3u6u!1Re}lD_g*gABuu}RVZ&od- z^zJ6{Ka+hNgWSIl$sgJti1v}jezkp)wIR}Bg29=TF;R%J*&dMI>`c|qpqWyB)o>3| zB{1jNOUk#HN_BJ?o=>G>REyv3+>d_SE^)Qma$FUb8Uu9H8E6*5P1Ztr)Ihj}MHBj7 zqH)L|lDFRmP__qXk{Xn}Fu!@vB&=XBCwqw~s)s-M4Sz6s#4+0S|Qz~A^mbvy)^w11@(~DQE98z<}yPP;Y`qlI|4~f3V zjfX^T@wNWmmsA|*r*lsAy`^yD$4ZTRDJHY)F5RYr+d&HL6%sGOziAd~J(h_3*s^_KsAMK?#TYyt!QEgX*)I4$wup=)$fjp}q>TjlHUkSp){C zbaaQ`g3Q0oBiz!Fv{j~Brtzj13FPpNZyxsi&>K?Cj{`_cw zG_umnB<)XaR-S46hKiMO={zswP$TJDM~&-d8m@FxYRF_lQLrD56zmEh%R5(1HpR1N zMEB4j{v3t+chsXcRq`XnX%#ri*0adh8FuD3zakh>ru#0b@tIvK40gOYLl|d9f@#a? zX-mWI2nI8u?ga&bT8zvMz&Lrjj{$(bk$nK-zLic~dW1{(2NbG4 zZ0=XnMUY{W{~K@&2KMsXm(5|k2xg}HXB5%3Gtc{Q{;#}Gng5#qUy-$c5ix&Yw!J;@ z;zU_~YW;dJ1V!&1{1h!baC9G9#J#Rt@BUTRa^ytsuc_`cHm_d7KD@d5(S8ARp7oy(@Es`h>?~O6H%f-69)om5-ndB^<9BdcO$8doENEeMIeVyfPBVVR5|lNV;2oiwXfA z_n$6(U1}d0mzYS>?9=d{)RcnbT>P=iLn4tzLePB}hMsW#6F#%9%u%I!{S(bdOZ%?x z^!y;d@HzPhdxt02IsL+7GfNh|{!{OF`}o&*A?|Cm>VKt)%XXgz++xX+(Q%tIYHbaQ zw2!YG?>M`NTQ#2*?`DPfQDkVr5=$K})iT3)KZeJY6~0tBM+eCVvUWoUqdm1*=ItU> zAm1Pfrj5O6#@w#gT|Kn)I%Y`=K4ww;#EO$#9=Li6`!KeN-hg&%DBeU?A846i&3qod z1Kpx@-Y=$Yu#GCqerV**i3*a+hye%@e!iC2zYD^vyK^1BV0Rqn-*}j{)Au}ikFTO^ zs^VU6O)YHckz~Uy{=U-SR5YLF1MDTvL7LKqOGZ+zH!91#a!>A#0uq=90t)f?4FM&E z%=Z?I7HIwd2Sx*1_|gKhZF8xfGUB=bhrB_c@KwQ}ndchRu8r=qYOqT<9{NL9MTqvf zhdSK7B_usi_IdXyppd$eWZ&XKfPZj%1P1iu{{=dkyzdbDHSc}k)qCZ~lJ<~o7nng# ze|uoJjrBOC_pc(Lx06OUG_=gG%)LmVo}^~-9)=b;aCh(ImeA?8J>1{6oKK#={oD&c zlm$CChmLLAR35O-Oa8T<5%#zZV2fgQ|B9Y$yDN|aQzxPSbq;cUZJfGe&hsR_cbRTk z-kB#lLdab+^dAH>ul^`#Ka&jQ= zrfzaVk2~b16p~=~rkw}*^!t6#`00=Ppm9p>I|YWpM5}nP1yw@+H}RCafv|` zXs@p8{_=-?wx-jN23Mg60z+Y|CM1g8GB@irfF$ermH4r_YPbn4!~j*rzwY{Of4J+r z8r^S8`Z=!!v_RT;C)?uCMj;4c&zA;9`grc0PKm5;Kt*3PVI|&?;eftnY zdvix`WxKhfOTjbj;x@DJU_C9-f`~@svxV_wPIIjR5&zh(q<=EaJQFZ|@$sM3Rw8TX zM){C-{@`J@^trXh3gih=r&}r3kq+>%2{V1VulW7)h`+hwyzA(!sHXXpcvuPNwRH5? zwb@zPuJ#^FQ}bZvWnPSygWb!kQ8j)e&pZ8WTMTbd=dp&u4Ndi_H9SC5)qN_-<@rAx zG|v=qHGfMS5c4$|V_t_35iEL6x?P#OkWL3-_i+bacgq?;1}T=ZyNw9q!}4r0jj#rW z4Ea1*<|`%Mf^41fh`QS#X6C*xcjWLPAYY?hy2)?oKg78lLIUn?z?)ye#^T1x{qF3^C1`xy4pH=oGUciH zwvX?(ByCtjg(0}Ifij% zB2eN=Xkh5;EXedrozkF(>*XuN)zB~OBYbtlzNkmK^afpH>o}N;5nr@=w0?x~Nxv4L z34SA9I~+8QjpYxkw16GsutO>sgvRqXib(oAw_jYTR;V`Wa=F+n;bz|3#Z?5Lw}+U; zIi`b~+J?S`?em~d^Y0Th*%W}6{O*|Xa>QT+&>5mdh}G)pA2ACS*E+~%Q+?9!BQRkt zBExsU3pSiv;03V*j?}*j1F2Fj9|3gKdBF!dYGm9t&0tf|WX9G4hNJ_1pV&p9#9e-7 z*VxFJ+!2x6UB2P{btC;@vs6Q^$6p?rO8vV&!P#xs_PeoUMAi=&1Ny}zm)k7?qnnA`-wkWM-h-0bNT*-5(qHQuB`_u* ztvhS5>s+~;DOQxu1VNfXdc@-W!2RA6 z-l1$Dg==Ju8$|xI{y>iFWiWcp-4EgH4*+_P%9!KR)9(1B!gZ`qnk(jD_*@S?cJEK% z6{h4C7U$|)4E$JQ;Q6a}^kwCNs7?y6yAje#<6L1#fYtfA(x@q79@ZT5OWA>}Kxdw(O+s6@S9ZPI@n3+< zl=UAfBK&@J@R(n1d?*xrJ6i3De>@>E6}7Xwc0mxpEcYMiix=^4{wnAots!`W@7nVm zt09J+JdZ2AXH|B#Qx5N7ryx>K) zpxOnS<^Mp!9|*iZ5~5s0H8T4UD}>Ww6=|fG{-)1P$H91FGX*Ih^&|Y!&k6M9`se%a zS#5#?$1NGx{bUUT-ycOJ1+}q_G*_HKj_2dC<6m5-`IdZ%5cv~ejsHvXpT`=#lSgjC zYi(C_QI1|+UDplM&O-&S1|1yNC*1}Ob3j$(r%>Y`tKDhAHQ9AO1G4k9+T8&xPoUWy z{!-*>>#djLVfdN`1JwN8P!TigGuLid}d`6%H zpO{y4gud3P^u~iK-2eBe@K`_oJT61Py3U?PP_OfO$GkM=5qg$B&he8H75`J7rp|R) zC6;X@kQcMVa^mmxS7_*2|C&rIe^SJXACwDuXK}L`BS8JMG{Jh}Z-^Rs&2NaB3z&My zSwY>&Tr~szMUk)0^ccvysKFqb5dqHLx#5-nA*6O1{3~%zK6C4@$M@75><6;#oPOFRJjF2nOeyPXyEH9XVs;-NKDyXx%%t@?|*W(FuQstjPAP6X7mQ{AM`j+33Lffe16 z!9K@^y8L2I?5h-Le(mucO2sV^9w24?don?3&K3F*llO5S{wIP#-2*73quQCW+u7`+ zcqhy~72fTwjwXC&_T|7+fKV#oUUS#hR*$^S9>K4t$;Q)(lV(*!9fM zJ;IoSyBA(_F9cwRGZPp0zNH361rwx868MG8TWv%CR@>MCcO%P)Kg&`ARoz;_DZ8K5 za8HqXwuZt(C)32MGB z#bD!g&m_<{a}h8#xwVESO`ZePcyMVNnwwc?#lDdOJVW^&ghmMaibZYUf;lODPkQHl z(x_Iti z`=9u;o2|uC&*%tN1OM%g?rV#o{T~-Jrh@8Ing3~U7OI6m+dJ6%@#A#2PB}*q3r_Oj z33)nMsxZsbgmHSyg9GWDvS*C#SI*wB;YmLbY76B54k%gJ=S&rv`s&9K;Duj|m1tUM z|Dt2Gs*jqWllJvbYTWj?b~S7{9nUTDc^Mni<4U^`2#s9-(0QbqwhSp4u=kQ&M`!|| z`D$Qa^R;SvlXUM(&s7^D-LyFq^Z=mnXEz(lZT3?#GZ42OwK&ucHv?xLl-&C@xp%7b z>Co)CJ^dY+#MANU!6J(<`}K}K5_o<`j^&LCA)>pVi3ke-Yw`1te4XC9o%bBO(yuk5 ze}i93a0H_GO{*^w1@gPF4RV}7rydd!yZ2L(=~UajSYu<)8pri5X|4ua)9!k>SRt}Y z|J=6SHGR&N=4p$4S7;-di=~pP>clowQ%KsIC`jl znDeZBXd2x+HjsAFtL*m4O*gd*rjPT6;?1tRyHIn2HN69o_f@0jTz5}!>3!3EY1kB% zrhIUQ7B3DpPKJK^wB_{67WM6m-Qeo;5$~Y#c}U(W#Sc_@TqLj^1iyXUsr!pRj++MI zZwD+1_-}Ou@|lM;i2S}fuma(@*ZaW>>nZp8KE}(*iI3$q$NpuN?N{r;4)&wb2X+ZQ zbGjG1@Nb-K*3~4pVymX@Un3S)VlTkc5J;>`<*H7BQ*T+svYEd9vEO;eGZV8L#gYkk zaLSqm^om7XDQ7{M=aPUAs9mZ9aRykKf5c*a5}0nj&h2@8ZvMAmC6M@_)E#0ZSuN(- zoU%D&A~4gPXpjp+$i2C1js32r{(BPhf|9?p_hC>9A!0||#4DHMjT9nW%_ll@=g|b& z{yvIrvd$<2DB1M?PbK38wj#InrGIIWnJap0z zenRX+gr9wt`nNn{W;ro8)XEFEv z&ML;ZSYQjnmt$%I1Ete0U0jcfQ+Cc)XNt(jKPc@E8K+8-R&$#`b^{Y|iv#wE+Dn>C zeo3@$j8TLkXMUv5mW2iG$B?JDylc0FZZ|*6KdLc8h<^LJJ|P^B8P_S^p&YtCdpC7?l9GkD7=tR08_H42!k!eD!3v(au$`ee%S{&5Li%+Jo(d zkN`K4ZNhULx?i&q&7NY${D82G?LQQkWTuaWf(E2z>K-0zd}s2hOo6h@|ND92-&HiHlAn>m)?l6fT-Te%>5#Pt^eKwOBH7kSx!@2Zh|T5Z94 zw%Q#_Vv6&t0p00ny-G(U!tHYjZ=Cy0_cfub?e|8GM@Y%VeB1e`Nvn0W^BI;gq&EXF z+IMw%=mfhjG))fxkYm#uKj8KVZ*>W$FS_U7+n`}~P6ZqRhaikhc#reKNwR%;{A$;UjhsgHhqR2I+x z>0H;&VNkNB5=mgUKgaZa}em1CUF^#)mu8k6-Y#GyL zjq;;YMVon5)$&_E)^LxJF3@6o@*>Nj*}KlZ-lo1up8gUw8FzcWAO@)ToM{0O5H6pc6$sG)Bk%C}x@JwZUH!DOxb1;t`FA4Nx9N zE}asI%=uIV@1rHeg};=UPQ_tS;8W1+R(pstH+@8q9-;4;D@@1!C}E>K>gVVqHxXbO zUkgd1XWuq^)dQ8KFYmKtK^A-x)MD>ZDFx7>$}0hIXyjGnAW)tNw0<(6voQ+Qa7RJk z{F2^pz^`~neitqkAlwl-;H&U(O>6zScu54)3GX|aP$+*MmM|YT_;S$FzsICgl-M-^ ze;gIao$hobj{oIzA_1O6bL7QT(NRRzl1s$#*3uSu%g`zUt?Td*CbAvaJPC}XBpGX{ z$CI!04$^a=ULNooz;{(4dXV(?OBUYL6gPa->qg<2?s_<|%w(6fJQJ2Xt64EK@d2du z!*ESX)J=NkUk-R+ckth3x7ReYERMcDhNQY3xncB$nk&8q?t6n{$k_!)1gXkW_#y_P zGeZ6R;P@@(Esgp@px;;})YB=yB2dQm7^8Bt8ihNSn?$sW)EPRL!7AAbzuQp{-JP~5T;R!MLMAIM4iFb!54@GxX={pid4v{?&@=e#zpL1J} z1KmTJZVGz#68>syHgL$I_+N^~p|(&zMgoReK5EG_4$KSR17V>!!^B`F&1Cz%5}U!J z6?v;syyMY94{?^lYG9tf;o$Nt%8j(~vz+m_fbzaN;T|lmE`I&GKkr(h7|e>Dyz*=q zM&^B}g`Ge=4dg#WC?#c%iKg`*6$Gex70^lK2v=&;2BkzYvjpm%z2lt5L0gY~f4jg2u+^ z_xx%4cOfYrT7vy95Gh*`JzEt8;)0-T8snpGQ;RCFv+7@nGJ`~npjFRvPJbq(?b{!})l-Og zeO;`dHN$biW)IH1(eLYQI?3?HLz$Tj+jJqy5_`%qq#8=uvT60wIyf>!J{a`IY>oW9 z&v-cB8|e`G9gK>d0s7JS-j@!B=8iT;?yTl;z^q z&J;T6b+ShmC(mu>&h@%R#hh>1jjW7aj~$2 z#(kLD4s<u_54KLJf$+AS zN3FK>E*mA+4&DtA%@l2B#Zx84k=JcI+MC47qnUzz&9!l=X`V~}F=jT;j}TdXqBMAV zyZM}JK$soY%(i<$c&kXdbyth_2f5c{S7EU=OwB#s{x<@c8^+61=macak*zMccw@O+ zJuR+)u;kEPvCaCTbzr9H#gT)x%k_!v4N$LF@inOIOP+()VIg5st9U1lgiM9gc>>YK z=(j$1EDmy}OYo$@7ne6*P=EIYjFH-uNL`w2PIOF&`b*B#&aQFFHoGHHIMbO?p&+ZK z3z(l=H1DQ*^W>I~;hY3-=2Bx1pp_Bb^Ik4wMg9HRvjT0(T3-kSnd2U%+Sf8Gt-pX* z10Qa({4TiCz)@g{!|A3WKkwr)h{Y*>_kUMMukX1dy~4d^lE{jdu(NmTk8U>2p8LW} z=IJT%l=JSfGa$p({3YnRgqi&!A4^f8YeI3C3+UYe_bxQpL1XMV;Y;=SYV}k9^PtOj zS31PQW+zQ)f;x0u4(YN>T}tO$2nPcJaxPaEZ7RI<7i?F)cM@~&7_+CPXmgkf3BP6? z4#&GoRz|FDZTqa_#B)BTE1L}xzs=DoCZCtS@kg+)PKi#7R@z*c_x5E54>zO3m_Jhc zS~mo7`FhX5x*Yb|F85x=i}v(eA0XY8k$*4JaKS~UG+d35IdvV`Il0&$F5q^S>94ki z+MJz~JYhwsrylOwM#|a~#y+dpz|-4S>r?ybxj!$yf+SsdAqbloXMhnm5jZ@qwi~FX z7L_7Ky%E7_caiJQH?wD<-^OKow+&fQOqj)A7Op)hIMHG|OX2E{-amPCjG$kgkxK@? zsYkCu!qk`EH5fZQ9(+ZaKu+ zi@aDQV@}vvc9VJ@6K1BN3mz*MGr`Zi%;Hfecm+X=%Q7i;%+#^|R7p6FHhrHn=)EOV zBfcd!72Y2f<&kEb&nOMsUmjr>Xk7%`F7daqPUf^I@2uGmGN5hmiDK0pF@LSBQj2l)Sp2%u9T0~-{>3ZpKQ18oCq4ree zlP<;gJoDY=iVfst zZd=tP9x~#x+IsbKK<=9Puq#gRy|7jUkyd>_Wqcf;!g&wxwh49|{}i|FtBS(_}iyM&T@-sg_9WFp^F*fU>MQ$ov6ujCr3E4 zZtXw z#J9Dr#h44a&~bKbHkQDzrDIb&&$I~aSlw%*G&yT*&Q59Hm9_HTuIZhO6M~M5qPOjh zxARz7}qH$z}jqHG@R)Yhs!%nTw+@A!9_VpU6R@RHc#t)hE;vj z<~V|yccz$Ui@mn{780pN(?XlYwKC?Y6mQ*qZ4(l&+HzF`b%L+O>yO^5$e`7VNU0Q52()F9tmF#`N6*lxF&>oAh#IOTjM{hv7}Eor`&R ziPm6F)>;pv6Dy^MD@2Jg=^2+ZW)BRMEMZHgQ%IZXC00{(ZM2bzIcT_p1>H2Ycid_c&v zhD`P~oNssp%T_eQl;RH@wz=CWL{4(ndPzN!{ohL*p`YAQZN z3c&o}@aS^i{&3Q_XIeE$Jh`e+C&&JJ6sOsGVIaP`5-+qiU?VH)tnqZ|G7Czg0?*~w zr4L3s`Dl&{1wzC849C9aR9jkITvURLY_-)w8{C!c(#aa#9@Bg4UE56-i-KoZYjbco zoE&sb^&2ZO(uxIJo8m6De5&|d`%^*h{J%Y44#a_#3~i#|pP>FI$fQ57($lMeMht30 z3ApOf-{?78WB*7*0%F<%y|D;aJvEwlj`#Ha8)J)2i#wz^u8nS3l{j!0t;cEHRKx5#Bs{Kh*OgdF-R+)fa zOH)PhNFsx(>C|wjQ($ooxN64cUL>rFt^jS73vU_Ph;;+bS{=DF>Z_y^9B?dR>dJ^x zx!zV8Cg-=^t|>hqMF{ro`H&Df9TT_dvnsd70*LBPmkwK}2cwPy+ZjlQXI%HOi{~9N zsz=5XCz(%ay(Kt)%8>^I;ZE8!%N=wot%sI<7u4VmdpAt(7$rVEHRbOkknWsrrpky9 zV$F4xKKr=6$D+YEdbDmFHIQ?G>m_W&AQ6x|n&*_%jJeUO^w{~Nrckq5(7*8h`i1Rj z$)z$hP^U2(Y)TdB;@CbKwQmzwX1khpdH!bjpviSch?0f^&;Vc0!n7l#mN7@E=_N;uc$~dU zGWubr$Mdap&>K?6-jz@K!PVx#*<)ePLorU9Yxi!7ABOF87>T@9Jclfv#&NHvD8yHD ztf|5jM5#ac6H6BZ&gi9>0M=J@KdZ4Od!E%38jPs{5As3VOooXM}g z4uRjg((+ruV^F&=*mZcg7mE=FS_RumPxvZ`9qbN856Mwl4#Lf>^?K_JYE(%Ak$=Ry z^s)3IcwO?~$TvYR*V3X(AY58HuNj!C8f?hCL~`$V5g?(a%RDovDJ=YhM04lUn990g z&30fQLVWd!s)2p8p3=&f z5EWXM3eGCSn$n(cqCGHJ3D8&(NX(4=fi0&YpF0<)2{j!uu&O{l`(Jt+1uu-E3g=F7 z&ph4cyxhN;bG#!uMt-{NA~EbWy}18@8SVvT`Ysijp9O7v{+GtKszvlkd3&mM`6Cu@ z2B+$RW&<@GVDa}UJsXeQU+{NWNtd}Uz3@5dD}|Cf1cl(sj0%* z-4D;H#Ual&)hmXNczalN!GD^3$7`I5!AhQgwtC%gRyT)m- zBbxk4TAx0%oFEjc=?;_NL8rn}1jkTXK1Cz*WSWuK%UxM5A4>&O&7kV!)~eURPZ!q8 z-9nJ zX@J`FX9QJ~eMgWNql^sON}kw9pOG}l_4n_idO)?6wgU2N&AQzmM-xg&Gf?2LuGa^u z_9dM9TLK&p7O7oXY)dj$_fDnw8c&QoW%&ghvRCn?*}e~hdR{4NVA7->bTjl|*A5?C z4r7KZT6upA6`aLvdbMjM#nte$;z?j5%X-}mrvh7)Lhm{SsESoW>_jjyexW99ILpNN zn8AbRXOqLS?x|_)tB{7DQTh(-Vl#AHt=OA8k(D*2>zP_aG6KPo<^x+0*@SbH&z70; z-fb&jk3c_*ulTWVwuPi(UnX}+ryW-}y3lgm@9o`{wN-1Z839*nHC9qfeubVeJckD| z)I`o3b}%kII683+*1+{*nA$6j^W1;J-%cbPO*j6gz|lU?)IqDbwlQrZ4nBvvW~th{ z58qY39F>FAI@@t}l8eqc<5#$ohJkjgXJ+)2ni|1$o0`!poN;j{_PcYX2fLe6i&!a8Fs~3FnPtt-5PFklaV`?2V+L!`Iad2{J z)5ehyo-d0!q6N|-!@Yl3WL76jirpO>Pom-dWGo9#*vI>T>J4PJ+BAepVXb{mc!}yb zlf%M6%o>g)azSdsGiZm@;q?k0otN9^F~F`6izt~a8(-fPHu2i0JyC@#Y^>;AXgLbu z>r|bSsp~ckVyUiH%i~5JGu-mr*K>NODjvmAhmeS#kq5mLaun&)QE*ouwtxgym~`&jczJX2Indh7>StBOIP9+x*( zBzgWPz_>S$chtmc+^}X^z!it_wG|e?RZx6Qi{doeE)m(5hM=W1zeIC#-Tcw|oPE>t z{kY?c^Q&CUwMY0m8%a$SWO{=^mZK?Usmsr(4K=(h54h+14Z~6U(EUW?R4q+jkU8p& z*l+X`uXmD4lUT@Gb}u?9h=_tWaAxfU2I3NBhae8rL+FTVpSgJuy2;o%xDuxbgMgc|P8(at@4ARfFL& z@;UM2*>?X86!^12ojkOT3`d?!k5BP7_#h-(kFk{!T^lp+v<}4|49l7v@|L-@!atdW z6gKr1=u%W0l`OmMYZ8r)?dO~9X4n^>zo4|MR*LJ}7f0s~-@fpTb#;h5p*Fam@pjBq zg}wGCK8QG7pW3Ceoj3GHm7&Rr#a|99EX72Ibf#=yd#PL=rJ&0UH7(_yA14?Fsr9p0 zZ|1UXy<~Oms6xwcbvBf44nagNy0*&_;<6et-`I%RKO;nycINlmALjgKd1*oHI(E^9 z5xOzUUTU?~kSf&^AbTXy?$3zboS&4Jlm3$PC#&A%PBcywaO$fu&b!LdYz%f2%LT+L z%eYJ&6dY4fF&IW^Q_@4e_<#b}Q|G)1eIlk?c07{VqD#`>QmZ8Lc6RTYQGI#3f{7D0 zHt?J0K58t>iTJa-QS&v^UC1A1& zqtuJ;c40#nmG2#P=ab}cn&FarcTM(c%+T|lVExinaIVo%?Zy6 z_}+w-2ENhHMgPh{(=ASN*4CB>shzN}F;m?=t%h~5UeBjg8o1M$o3gMzNCE^OZ{duE#7?4rCQygr)aUTGEmhyp@ciOjW|+o=gIpsd@yiRvP?)J5Ts#lY6Es(O147CZ;xRi? zj+Z>p(7mAyFL9vm#R)Qy zNJkb068ktU<#`4DXojwlaIOFmtbz-2)yWozv*zy0Y`7BS!@f$_A_ll%i1BZ%GzSI5 z)v6T?i8ZdgYdCN051cq$WE|Ms0t*Bi`J=C7^pXmgh9*nOpw~J!#B;>&cH2__c8%2yyNC8(6#=#_YMHD%@9D z+)K0H8m9J#B)k`a6Dop;%zh*w;?|H|zdC@-iX@iH@RNbrLpg9jN}3g&6eUf&?}uc0 zh_C^v9(hJcGxD{k*t>p*VEmI0-Qyh2m`pf72bxNZI5#cBzHxjmdS=Nah2S_Vj$*`z z?>nMRJW5_G!X}9eTjJNEF~GY^aq*2YK_GI@L66{>B2OZlRia9)@Z&Ne^#KvY!}PB* z#fYR`8d5(omM*wqB%ibN*U#mTT?L4>*s{{$c8~k!GKW&cX@#}SMv4NT){y5;C6@%r zn_zY1@V6tNU&?(}=qBV{$VrU<&_%UD;VJK_(q@67($vIazgs=diVnBDm;jN23&iLu z0KxI;&!5mge=S*BR)+jsw&EL^z^~-#c(+PGlrE*9(&M)d>#IWk>X#QDhgrnCkmNn_ zcZcH5*C8K_XNp4!zIvQWLbO%AE{D$h%EsYfVuDTeb)Okg4EiuKn|SfSw<@w|fY;6n zHa1q?8;aWRWg)^>XiyDaX>gPJ4)vLoa)`4+ zPz|Br2kRxZ)h??Bq(-t6sxY>Xw!MZ{ErtH#rpFIc+xOo+HPIe>TdrPm1%kXlrBo(4 z-hB~=BTCMUsp4M~z@xT4nO!GD&-~RCB?3b2Gn>1OEx6J1TT{$=4wG&uc@B5O2x9eb z-}iGj5iJq04>O4;D~PjW#JMMyR`U}S`rL5>FPFI@glc|gYXm_9GQ{3*k*iyEQZ1tk z&`Bsn%#J0D5iF5^=%bLjO*;_9RtML?pqMc`jmPffIhjhC6YWLDAA}2M3Uyn=rju@? zSPh3(SyVu*8G|);=&<^)Z=pi=(56S&;zyBHv7r#x1T2Qjiej@atA85LHljwD2=UOd z%3vff^KW+1K}-+ZzxMq_?ZgX&tusb=OmFd*sj%BDbl9`wq^hvM+2Ty|iCfFq5`I!4 zy4v`pqPKzi+L8X;KT_=h$Y>$8+-bdi!RuwmG#5HvP2waP`%S^)E)>)|5~)Aw6GhIwAf7aI^PruX z&V1E|k<4`7MAsT{KUEb)a=~ei4F$t1JvmKFA9#QAHL`amJ!ax=S7sq5{54Q73!_G4 z!BI08&QKuP7ns2?FHJI&le4V6^6^AEA(+{hGlFs{IbE1k=_2UTiJ^A$QLCb1q+TTj z$})3OZbt*GR=f`XC3V%#bIJ@h%}QjPawWkQhY_J*Xs6-YU0d#ROer}u;$j6N{=TI# z!AWZ8A~cxkga%mpNQ30KK(cgdukrFow$ZfrVnn87cK#kYjs`plB0jaMxXs+!#8tGG z5uH$rz1*+6sRpbTr}n2bfwAA ziU$CX!aw+|oK;Z{kmxk9(MJ%1LoBc2Z4|3`e2)pTbNyVal|*VVZO8Vg!TOy_=^Ca` z3J~6Gy9tb($wBhU&7uvu4+O_)M8%$78~dzShr)q2S+ohLF~38PO~3yP(WY$>V?pCE zF4CYGyv13ZF~bcFf!B-d0gYDnIc6+Gz(CP-8l~lH^V&DgzKocY%ND5sUO~NeAsXa@ zf+dX5$e4f&ti|@6y0T8gNw1p2&P9juHDsjk_NMZt*f72#KLv^qhO?_J8slJnlzx7>9$7CW8FqimE>Z*Z=whfVeY>w^9$3F)rn|!!o1I-L~<|( z%^Vqf(R@lBRJnrTX4WDHQU@R;jmP#Uj8m!^d|iZa%JosE2PF!^hV>St*&(>&eE(jL zoBb*JG_r`H?gOax=ZO!hf+;}Fw+F2sx2@K{B+3)|Mp(WKe3-2GVAY{8YCf|?11W)% zq@tyV?^9xSl3G9@iU`@Bgflsc`yJ2_Nth_0W4k&dgE_mBst^t;>h3%IRvFKb_&DIJ zLJ~yrJKmegWEr#(z|h{o&PdPd&y}^lIW#moK0W@Q3l;`u7S{iWW&S6Yo10F_)y9ZU zO;+FB$iRV4(NW*w&)?!!dL~A6s%C}`rub~EOmt#KW+tW%_>4^SbOL4$_Hsscg4R|x z)&L`b13o*Qlo7xLbR!3-Pz7i@5i?5%BRe_~OFai8AtM88LnAstYfEc8MH@W>BRU}? zCo=;h1yKQLUS8;bp4BD&bQQ`2$#Hw|vIW|yCL2=13_*^+W$GkPI<3HJ$zgIz0L>^$ zVo{QH=oA0PV(5TQ@WEUlcoDt?j^j^oeqpU&TSQMqpDEeVA9Z6DreY>T!S;jZpA%G@ zT6NEyiJaG8yqs0F%OpUn(fH=o3=*IvDBG|}O3+tdxoqv3Zy&uUTvnRfgTwKrb6kmY z9R>e-rL_cr4+*qn|5+^c7BNq_!2j_+)O?jv`x9-tyI?j!m z^}x^SDqfT%rsz#!Z8=t_uFNcq>R^2MAY||(Ov(q|g>mYuI)?Z+iQswTehb(!oV)8O zc|IEKr)gh)zHz$hHd<`TuN?Fy$@Fu-=2_L`@bFrHyq&bF(5g4wOpt`_)9EnFx}dDV z2<`#1NW$CeAn97`>Kr1tITVs7tuf_w1uCN zsrC+pcbSq79J|STiv8MB#(t}M{V6M1+eTkR2izD9JA;c{u~0^0W|o4yFP22ozl`yI z0Q1)fn6Oh5U+?_2^d$72QbFiC?1c}*>;cocOj?L~7BHNp6m&v;W#V~ggn|Q4G?OZ0 z_Y01+y?oOU zZ~XCA=Cz0s30MoOJOf`MKh~u1U`lEjQ(NMlLh?+T!RvcE z;b?M^JcUK5kcha0hO6%Jej$vaIm@K{EKwe#dMbZLYOFljMUxiuhYuUDk{g#m^6791 zmVv^2(wS`eQRlA=9-5f%r$uVrRQ(zm%qNc?^%pvSam6y%-z*cy62734LXFjUXAC3m zESa~PLobz~G0H(nE|@J0ZM>fBt69}Aq@yERn*N_x^VA%Lb|MmWEG9yu36!3bCs*3; z_g{MXJf9C~V8Y^NuFB$tB53VTMk2U$Tu+fKn4b>nBeug2nb>&L>z&W_j-%mpT@6p$ zAk(ru4-Gau@!D0IUEMllGE)reWU1|>=fAN2?jK)HvxmF}KnTMfG1GonI}0 zt>+rK^IXqPR%&v{CZW>{Xg~?Me||K<1@dEIlzfzwcqCK)!Li&BNsve%4i+IFs~YJ; z0rOe^Wpm3x)olU!=Ivwkt3N@5d=E2TEhgMmU_Z%nrK-1`d&eslMHE&%=u# z_q+4*r>;9QJZ?2ivbb4T?lT%tU1yWMu#V~?f*$8kJn+D|!#F&dJ@SqkILpqe)n46) zj$*sqE-c>T{w&`U&sdySuyJFyO(sKLZ&qbeVtj%#J;+6&cHQv?|Lj{qTx1TFxzneV zulFJ&xlp9Fs*8exfx>jK_L}IFd_|ZrL+e6LgQI4bue}_`D3M}+f1DU!W3635C=t_G zGIdq$uDVxZc=ZV^>Q*pEKaj6!ka6)kE&^ITl|h^&Nkn+G-p!A}V$1-_fB24Dt*DMF zXCyqSxzQ9PN(j(@cObY}slbg%FOn3GtP~`Odi$mJq}*+#yL;c@N?Y3~gXyFz9(wU4 zC32p+*~5H}CW&GvF5O*ilsgcC!_r`x_hYWC_;yi5y%1x!=odVw(KDaqD59?hD2rmLRkJq4XEVF@aVJ*7KZf8Bv7&J2q0E|UH-I|$mqaI&Nu&WjS+!`mQ) zbEzXlME_o=G?GDT$^7w!Epdn3NWyWdDk18;lx5vH3sb8!m|piD_1lLJO>&0cxbsUk z#RJt06tr;66D%#(hjL%tamHJPtGHfOv>mecMe{v*G|1YZ&jbGA)BA8JGU!EOFUtEd zcG_!AhqISXFFoAl@ozfiRN}Z8GH~X$7{={Fr_1Vi+MUKKQ_-n$X0z+)Woa^Q8HR*7 zf&;st){%LSwrPYJrVfraZukSHyK*C_+(#Jic}DG#Cm96U)NAJ|#;*q>(&*3Ep=cp+ zmCGMn{)YXiD;HN!ELa>=TW5zUONU22*>#oc-W?M>Ox;vU5cjxmD!b5qt&Tt} zNP)-0z$z`FFlEM+5QW3x(2ut7VIJ6l5QW4#fw8FiMRjRsRNfW$i;Xh2P(>q6#fj#m zvQ}pRyPP3;%dpt39pq4;MJDx!SPa)^?xiK4!49(_-u4+-?x}cnpvGD!H=8sj3W=od zo4GWMY!@fWPu%F1 zgZw~CtGtUSserVPBL0iunfIeW)D#yUUJWmwNhCw|T|T=RXEAU0g9&)l2=B$`X5&+@ zV-L8F8ppwD;7RB2d=whV%vaxXUTdo~LmTT!P}c)yt}BjJgGCFkl7YByrZ7x7pQ3kt z^yt!6WoSP|8!H-xFkD*MA4YMGqDX#JQsN2Q04hzy{le>L}kRRVYuhUViMn@;dvrr{jwyeEl)3o`cqv=1X9K^)8O7|&lB5hfd}9+aX_(m-JWHz~dg zJsRg}C@=0-PRP1=x6yOslJIKi$SYJJUd8K4&&i^rHd}31Utrvk*fSYBvtX*n7(vTV z8i5*^tWO-F^&O-vtxD}eDpWkC@vf=Wt7f3g*J6}$Ki>Y5ZCwcAnZ6|M;z>>5P`0O{ z=OPt8CC(qft$~LY7n3=~S84tB)2qkA%}v5XE1dqNk{opEpqyxocPh=rfxhykEoR{i zW`sa-^oS8+GquEDt(||K!!8QQ1x{1ZZ%h}F;=q9c@_;`(qVRcVeMC8!MM{77m;C9( z4uYy)${=%=s|9otkntZrn-zZl%w3X=Sx+tUXDd8>7U&(r&QTGs#{|i*em>?>g!HY zJEq%a$o79cByDbm#)o^ zoFAb%z_bWvZL(Ww_t)3cd74cuSF+wS?3B-0Au=d?|d<(vOT}GQ$Wx2Piee5MMdrN$$M9Shcdou$0YmZt!1X-B>}}nAh)(%`ncVqFynWTlJ)c|jUmmk^!cSgG^8cf84FC-;C0rA>}W22-g|mu~Ji8F98~^iriyRLfVNLYdvlye_1*e9tX6VBZeyW_=(R zEcY8fLl}?rvF8kb;&7fH0J%n5-%|CM6HWzq$t-P6cZ`6%TAFMA)7spXslCMFHoT>Q ztqdVnK{BEtR|>Ic!mK##TLsJX&+o71vIe&MpPh5+ogaQfMWuOrpKt03Lym;|9B!k^ zqFY{cN|jDNDYqO=XsJp}H3^h####%d5>iz3ljvNO0{(uPx|oKp)>R6mbyGV#D=SYo z=niX7svhr?rg}=q04Pjek71%R8uNSSe7K(OgV4j}Xd?ZQx|dgIeCkVF9q!k|{S0WS zTx~=m+-_Sh-McBXgGXsh#@6AoNpfZ^%jRTiS z6-lk0`#lV`cg8C$4osEG+uIAXH~7|5yelYYHO=+Eaho{7R=g2D3|6w6^4{wg&$*F? zex_}+5ZrwZippwWVb9=bsI^vdiBpYtH$>=Yr357FAc=YS2FbP-9MQg~yYVO@WGwM2 z=$Tz3aPzeolxG>xaIMu>xG6=D)%p@K2NXMTHf?5qjXQ8!?p&$$qhfQOmkoonVwWw!cboBg8hC{On= zKg-J?hk(6I*ko0J?saBw_|>ZBQv8ht-^0*s}^Hxe~p<0Wm%q*X|gyZ8iR!1x3 z^*r?HFb^3W;nEF?GMmfyVVII;bIBs5WR5x$3v$CbkO6lQb7^yzdDoLI=$HZ-IO)vy>i78aCfs;ag?f_0-FAgm`?yehPeY5r zFvaZBQ-!hZuDR#PaN%;VbU&io^7@bhH@~wegC_&3muLFB>?I|J=d33(iZHF?m%X&+ zzi@Y?LY}7rJMG-V9m|xOH?gik8OBpq8cnORZ5-YwNeFmS+j3TrVpZL&W~Wq!(KIZ3 zz?WWi0|s*`v@svtp8Wd)AV{3x)T(lHHZEajKSAa9yDO_`@#!vcFffb3ud_10$bQOn zWSd}e8cJ%&;@xa1fd*pm2xO?PxfWj}@gANe_12yp*fZc%jf%aotZv^* z6%Ad6js0XCed_z+h78=(hWIy#4Gpb{S zvxgJ53nv8e+G%d2Sqyi%-%jG(Fw-(6)S4;wT(J+xZe^*5W~r9nxIcajv;*el{Xo0@ zauVItk;1zaLFM{V2eXba5$QgUsXQ5RXlXUI=B_!sx4jpqd<+c(&9dmnII;%^jHo{+ zakkmBdth<4!>`HkTkEuTGrlbPSc}JJGw?D!Q8Tfcfrnip>G|^erH!A+{%oP<9PT>A zDi=(f<$Xya!)3esi&5i!M%CNm`S>32O!dZ2-j(e5xA?cp7jyQ-5if*cy^xeC0yuAZ zXDX^`r+25b*IA0@T3kNBnzU{tlX62rniF`#m-V#$0kWCagP}Loz+0#x_gz~AgxRc(s=x_KYZz`| zI0onGY}N$sstUGyb;2<1g;1R`O8C-K`QuotK}joQ`C!U{exk5w4 zTJvcx%6z=;=MQsT_N!Z($0R8#I7yY43p~Z`VX#e4OP@%~?wQGtD@B5Q$hh;Yt}u0l z@!AAI+QuubKRaXT$-YFQyV?FfOwMA<@T8Dkgr&9YrEu@OiZNtqbG@6fS%*zQK*uc_aoUwNf5ZxWw zqhjPr25Tkldjeoi@>@8cB})1S+x6D{4l9)0oarpzYB6fJn3y4W9n^V#acGqUq$0Xh z=oTCr9xX1GY&UU4z;Wvk1Ss;9pf*fCbC$MUE5v;CiIYsT)^;DK!m4xy+&T)JmB_Vx z=8nlO&n&xG_;s>$KxgnHsPD@7X=nffbf5>u{NwPl|4<+^TY|eqi@#V+yTbVa{GsP2 zX66+p8|jWycpa^h3*0?elr`G;%O}lrOw=_IE*Hbc>+@Ir#i;vosxJp;HDYmhYiT=^ z_PS34mb~w$Z=&-PbMqF8b~9|78y078DO7|Gp%=m)RP+7a*??leG%tOYcdf2}SiSr` zV&qVDg2^}(+G0l)EY-OS4rl3Rf^Gz<)oyARXy^L<2@v?z%7kOJ$$fcaw)szURqC`M zzIxI8I4IqG<%T7n(sHeOowOT!Bq;X^Uz6Rf=<#&6K7J1Dq!C*C<<9+>Ry z<`~dZvA&4uGSSXZNY2TTo1~@Q>N@vG(R&$cu69F1lc}|}UMV;)WOh6oR#DAbuGOhv z9z#vfwOWbVhKof<}pkC5k*)A?uU<-;%sK0{q5@co| z<#}1W?r(orT;);^QcBL$7Uv122A7eHE+j&w;q8yY+t!rU23DK$EQA+A{`F$o$7I&C zG;LN%2Wh^&?Sdp2I>ilX4VFG88t!I|w6VV`5bnmM6AgD$ikxTjckz&0`A4(b`nAY# zZVYsHi&K~!k$h|Q^V=Tb633MJt5dRCuJzkyW7zXGDsGvMf1`QpuMx#8G zfML7rY6STgw1$~xPoi0b+Xm~MaeLU*aZrQ*>s39b?iKC2u$1uIC?Ea%jhJ_{=SK6) zwOvhk>I&OL<{C?l_IPxr*)$)AyNgwW&8I{9n~2&y*@8Z0?;pPxoGqMcogSCHnX|ka zq}FYc616v~&t3|%D3v(Nvuj6m7nYSz6_U7}dpw@Tl|z?HWSJc+HjBq?3%@I>#2byI zGUYlK)!55m#TViZ~3fH2$cyOT5+_=2j`T1mz zRx=(Mdc1#Q!*%fhkpz96x;Wu9-Vg^Gl#tr&Vn${3(iN0}q|JFs$3LN1UAT=)Iy!mV zw4cWT5V)h-KP-5hWKAorERzyXF*CPBw0e;Uu9?a&8?gWxc6wMLy6qM&=2n$IZmDJU zYTQ&s=TYH*sI5!US$Wu5s5Tv&T!)M|(`My77T&L&VG8y$T;nM(I_*Qgvu~Q(?==%p zK2v0PjN{-1r)T(K<6+Z^Z+wBHc71vX5Oz{?Q@_f5Jb~#wrIRfB z!p#DC44Ii37%q$wR=5u}XW`Yx`QJ^b!&WGb6v;fOo5BZBveR4iH;J0J6zWWVqkeyA;11a!8q*G14}GH1Cu3n%Aufz~6u+*rk@tQV8^wt-S- zUZr|FK1W^l#rwNz_YZo{!1tS*-(fz5q9b{N`yH-D-yaC;q%_=zEP1bHwockSl=r&Y zu7GrB#%zU=p4tz#m*W%9XlQU)W|&GUAf6%%8%ih5EsWh%?X>yJQ{kaNmXpsTE1a*Qut|L@cTY^Z1zc7_N3Ghwj4Jso z>fspEl-Q%Wirrltm*ASR((&T!t^yop`j>wZf-PGu9bB&TsrcoBU(Z@el62FaO~B+pa#H+`aaK!$k0M9ck6I{o4Ii)b)168n&mUSfBm)5G{w6;H)r0XIfH)Kk7MZ zh%*g1i_bTG%;WMIq*BUqFfa-jybGZjsr&ev3kFkvDk9wCnKU<&Sfk5YXv_I@t(m`UDc zvc5l9x4)A4M|oVuKh8Iv(_v^_2ROd5IJ@B6IlO-l#Qo_A*t92DW8G{-?eVR7+dNb)!<;2&6@=T7UDRP+gx^lA6V)Oe@gSX) ztXP32wv;zaYP9OKa9sSQv6(J2*&-JKDO}!1t$qp4qa}2cQ3osDvx<1?_eqL_u(&)H zZ}n3%g)K@{t9{Cqxb-UtE_XrBZ?#g+n_exfi&5SdZ%)z$g!t~`l6)%GA(ahZEuYcG zMhPU_7_uJje}%~k9LFf#`G5vB1c{#=;%*vhf}HSp_1b;LJoaWTd#$Y^TQ8&p&eskf zEwa|{BTjpM+4JERr=}oUd0zb3ZA5b_yY9qxH`xb&bW@k6tBu3t#8fu1hNE`5DSCWq zrK-_nW4T~95^mmFt+#n$Q4Z52V|AGKD{)RUVk)ujtI#@C&s>%V%%9>SENo6J&a0oD zcf3D6Go8Fg3YzM-ofF-w(Mi&FW^<1-j`Z8RytI6!Zm~V~oFl}l^mNOf-#Kd#J%vPr zGw*QBC?`8hwPJdau>SJJS>3fTpSb&UU$%L(8Oo9^Q@7PX{kpkb`xCC@dJ6ySRi^8_ zs>b{Pm4!7q@Ze;ENY4tmDL(Sqe1n5~H?w|aQDw^{f~{0T;xN&}Xnl z7+_YJwv(X$S~ru=Yb(xQkJ~)mptu4pyWJ%N;0Y>>usuY=^KjlgYF_VLYIM{pSc>>m z0qRqBTuqMZzMMQC4SHqrKAlY~KFyj*#jS8|u0PAFiM&6p&zqo^w`a+8?nS~1O#t~} zVPU1ea(9GY-k(1|`CB!6@!TyLH~|?QRa{Muf6ITX^fhl<8;N0ySgE-Wy6!nx?`W_x zn)ex$<#{cB&v*yP!@q3aa+y~S{58aI1dnpxX%Y3J_r8v9Nh+nV9#?nhdl?OLUYQYG za3%gYk_U*1XtAwlxbD%VYP0HUzjuA1qB=PD0_^Y=UCoivB)K2vV0w>Vlk8xXwY4`N zR)}z$Egmtxn|;~H`ZI<(`-FRb@i8A}e>dr?_yJ#y&16H=L`vs^+Qz{2b}#r9tG7Py z{UwsU0W#xyW(j=rl#vNQaX+9lUSd;OeUpPDFPk-jYE4+;6tMfIdo%^Fx8=FfS zSln_OioJl&Q0MtA@p&a0b1R#D3LVwd($Xuh?$GqP5t*d(%KS8qQmoe`L7xHBjLYBN zSiFkoRIxU7&PYCm)v0s&vk)<-dE)K*H#lxOcfy}OXvwU#-yjrl8@cfm7k{kqLI9V( zZG^-H%~_8Ibb<82LQ#K(vSM715TICmb2Kp7VWUcrv$rl?|9{=^_ig`g!pg3!zw$-? zD>o3+MIi_QG$a1gMV(HA^a3>D>ly!l8A84ZY%u%^oz3a+-sA84CQK{}J*%zDYNKXCSiTl_J` z!QW3}JIM%I(csbeSJdAL{|;@FKz@7SdB8w^_5ar);Li$Fj_3yHx&H|VS^j0&gDKEy zKmRAm|1RnO{#tc|4gweHFI3R~jm`f|zW+I<%qM97FWUZXj#^%`l)vCN{+p36H2AgmDav zf0S|mg)r{l61W6qX^WNbU)!Hm$C|4m^B$|9!u2w41>9k%Z{*x0_+-SzXkVWbjF}B8 zTNx+<^=O7=r>3W=>rCI6D?dGa5i1BYT=QbpiRJR1_l9RbpwIf|RnUlsQ<2%$l>cA_ov44lh$#382{a##BT?Gz->^sHh7JcKr)> zq)7%==V$_0Ej9UGO^&%0rn;syX)*^99fj)qR<^ZxC)ci+@0Gt>wD!9u$q6W2g@vCd z9jvI}&b$DF>Fs1|(IxFT6wi#l*i3Q%tdg`l32Q5|`I`I%1)8{YAMp;~ZHH$)+^j>m ze!^pN>=`-eMZ?futfiVTjkCiWeHnGoUK(#1+#T?6 z3VuZ#KIbWFltc+7CLcJt@I}yxfE?wCLA}BaG1UAO;4IR8Wy_OGm3Wf~?KnPM%BFSc z@yxpmewZ#6ZB$2(yI>7@ucfN9i;2oo28h#m_H$+Fu;AhiZd`YzKkztK@x}&i=snT# zb^qq&VSdKTm(5a$mMi8HV+n0r=3rd~g?bDfR!)#x9h3;C&u@>#{528*d3-PtkV_)_ z`V%^p&eZ$kS)QNJf5FYaIQN(Ela2X5xykaM-28vkeX_H%{fF+8jqQJ<`($AKL+}5O z!p{sSEo9N>!RtitwR@Dlz#@ifMU)-4PCqVK_OqfXVfmZ|6!&vX$+AgwO=OE1Q>hb7 zq8}mnIlkZIh4_W4i570(xLToBy4Q5FOW?B3gWyh1#(1yCR>sZ?%4f96L5r}#8&F>$|F3EU*-*Lgw`yy+i z+Y(iqps1QQ(E%MbWb=N2yQ=RN@J|t0953%JP6nUoQ>Q!4C7pA>mu&=aU8{Oc3gBd`*LcX~{8j*9?~y7iYYK2QS0$Hba9Uu!=w z|(>AcN8AGWmkSN89Zso5A?8m9Q)&SY*3Y&dqIPwdbWI6Il)k_Wb~+NHe-p ziq_E{DX@6uXeGJjSA!%y`ES{?T>?k6py!tPZA-5klxKqrCr<~van~}1kz0o6E>UTR z+z(RZpPt+uVU(h`4hatT2quyz&mZsoLihSm$KM)ERlYv)kfFB5GPL8N9+4zPi^RJ( z0W(>whPNJJI^F?qZR*#16MbF=-Uh11`mO*d7Sb}FPD%KNktTdlmKzgNRQk~iQwtX5J_^nq8!jr9tT`T%cZ z9|ieWQFxE!k~C=xQ!Q}Xj3-kj5?@avstsfPlejRnwOK%La(Wsc-n0u-M;cE(qU=$`Y{HFcjPsKsC8$0jQ#6LgHDfJz)-wH2_%kzM}RNA_xX}* zXlGVs>r4dDi0J|kHH`@My1Jns$oHKvLd}7lWq%6~hN$mt&F-V@783GHc_sUTt_(5y z&s%CORMb)JF&J8Je~A3v-Jr)fRf4a4odAIt{-AlUkF!50(35sK^FWvklBhl79X)AO zM^tIuU*nd*&wn2{?pyu@P%QGLY_!ZnllBMUfF-O;(8-@UWloKm%d@>*EO(irrrLFS zM#hAmcXi#*35%N+a4#Umn}xSSR4n29UE0q-hkkB-p6T~`D_F7{@)gIvXb@i}{FRI4 zFC9|S@L!ySn0-K?E;aP)<=4B*VRpv3 zIMD84=u1TO6fsBnZgg)9z9N;(9689(!X+PTRnEA)o(3Epc&^8~d5(<-AVWRcJpDTq zWw+2|$K>o*p-zB3Z@e9ZQt$=vkc0t;bf^!WN&W!#PQn?$rajgS<6 z0i9$#ahjfs+i!^Ib}pxAauJEXpoT)wobcRH$G84HyCMr^8NJODte)>&f&#RmM~-ZE zc_>I>rQikzQOSH!j4l=W5URu}v_rtlk9)PW)HM7iT%Z{F%R^1(^j)5T^Bb?iK$wy^ zg|AWvrL)mZ6A9KEr#}$pT!Wj z$nVwh2P}&YD24^>$T4l|4cm+7WB~#?ArcyC{zIKqu(*iTf%CoNVqh{p9v<&o9=)~R z&#cCURq)r|&44Yq+re;K&_Q~BV|tQ8ZSXy~JWfZhp#sDaZwRVxEg*LtKse>5u>6uC z+iO)?%th^i?PfSmhYV9Z7Z=nAf(~fy?4fU0ioyMy0+Z`xKbeX6J3jhC!-}45B{q-R zd^}u<`l4i8%$JN!JHIZ89Gn!1ZvQQ)e}+92tH5p$;FL7{AcVk>Oqf03n2;qFUZWqd zFb*6QMHv!p-V1mfdG&Y|dg$d|ZT5+G6udvgKK05@D-<_IpM(LpS8H%k-tp_!{b>4vXPpXAHM9qDbj z(_Oo#M}CRU^gQ0!)D3B}%eDw*_0wTK-Ox1I_Z_ZF@~JE|M!L*t2SJQu{ndv}V-nN6 zPajGIqC~vFAv`@3aNjyIQQ*T|f}Y$~@wRNeqqzn`bPy>9Z%yMrKVv;dyhdmf_*S{Y z-d*;9-cWQl%Z>P915#;h;crgBs%-FE;XcF7wq~Zf?g|jG`~`?_4(iXCRayZjxI6v( zmn?Ys;ptRp7aC9d=V8x}A+Ft&Wu9`OLRYwoUYzOafsjU7To1?huO7lLKk@~3aJh|o zt;vAt%^|8YrDR^}4EA>ee&R)II?r5s+kNB|bV`Sj`S{2qxrOC#*JG|XeRZ$Pdp8Qf zReigL12&Jlcp^SRCwpA4@Qf6CHF#O`(z`^6E_az7M>dRayT?xfR3@;ex*Y5a5m^3^ zF^d=_u`$N#)s&!K!A%NjTigWc;YQ!E1vz7x~IVruOrZ#%7Zuexq2u1DcK3bydz)+3U$>UB~Ec z=VC{Y?JdHxsY)+T0N*~z{XV0e>5y3`W{Oe9G82J#L$XDB`QdrpL@{J3A*IcIdrSvf zO88+!ycQdO|1tMyQ=Y?5ZIwR%)71(Oa>6M>%VXLpaEXq;oD6Y9OY8|bK`Q5cdq5cP z!sX^8khJ7ApqH!jJQvZPq4P1OnNw~o-&FjlNM9B3+SW(BJGyV$97c9#EH$cI~@4%Mz7sj;hupd;64n|^a)LfT0$@zs*uVxn-}*gc39UxaaYoI>d{Hsm)cB#PDW@ti(4 zav1SPCl7v;Yby2NUggQtXQ2B|a5+hYTno$+<@3ceeGpRsLI3A zC6C9HXOqY}lRpBYM+Lh+%U`EG4yMBs%(+AF2;LiQad1BGhae3y(VgOM7hAnAmgYNN z^*nW``9SyQsqYbF%Orv-UVI(4qEm*&ta@Evl$KFICK!**woODyzFA7Xa5TO2;r0MR za66^uRyI<;fMm~QB}}6dh?O2M&~ME_us;FTpERC}r7F1Z1OERP&KT5{Ot& zIBdBu8FfCsX<(Nl=0cb8p(=xVNYlL|m7N=!`2t6!H$@+!Mn}R>cJ3W$a({K2dWOe} zJF8=qcfHNQ_MGk5sTm=V$)Iz%&A@|AVeHiW7Dp0Q5Tbr=y+Sz+o(VRRxPQZ9sxEP) z=*YsN8`6}2+pTE)+3j%L$pJb0(m7E2fZZ`a)O*0>0$7L8#f{sY^j7AqKtgrit{&x! z0xQ6GSTwu&!uhVqo;4h$7V7OZK@E$Rm>+#1m=_(A|8^=7-xDx9DF7&na)&f{X-5We zh%RefHT)IEy4VYEx8Io2$hK87fG{BA1s()mp^GcIKLXz`@#tgk_Z97*o>|I|Lp*It z9eOc5ZbEmT9TZ(%uf+WcQ*D1#t7+kqiEWZlX1I}dN6S%LIZiwEho*c@q`o8TS<3Hz zfv4+u10!tiOw*#?u}!wj6gta4VYP%Z4^?EnM>b7@1z$HNkB8&3`BtC!<+_K2`cvF+ z%<#iU-m`>Ke}#&TM;Q(uHeQ1rHA%7H>$;3;efe`o)1Cd3sm}yyCR(fbnL0~ax?)RL z%P0=ZF?jCH6sznCvby|@%_bLA_AQsmYIh55Kj`Q>oS>z=Hu`jHgWX@Msas==Qv?(F z>F5%=;xUV&slF~7JZT~V z*z(KnRuwRaj?#WNB}Sm-%y0{AZfnA|OY3Xq?`5bK8(rV3TvBP@sFL4$xgIIIxy#avM>tjB^agac#${R3Q0sbyIPIk)I?`7Uu~U zGNwq4`b7XWKBQ63nn4HE&Xb=KFCe%wHAQaCFMXYrQ!E@MhfA1x1pk+DNX^ypVASPh zVu=SgC8%noyWd(l+_WPbN!bvKu8rSrAr%RD<%K`ca>8ldSD=H<8l((k=LY-^-t-F` z{MMJu>F8k;jQ^6!|3s)?6aA1IUPFaKsIWz|F&uOzU_n4@9i9lYKp7xIMTXGq8nNhC zVsUj(z@1p+%>Cq`yUZ)sbGCOnj^}-%jHiHqS)kLn{)F@^+1LznDFvQCm6^>-6 zZc?1t@#*z3wn;y@Tjl*qOFz$*`z*;GJm!#-TK>p~``T{8Ra5I3t%f6lO1KVobib<|j+-_iw) z#h*&Ts|J<4=e)kCxaj}%*VwHv%U2(k* zFT2xd+n;iU*=$bKT2kC481b`>?bNsS-|I!n~rIXfC?k8};@VVF>w(}{4a zNNSsFDkWB1**7(_ZJ1b5ymtr4MxhMR8ml~^{o359K=ph8eWY90ry1PXRQ7ZTBY*=R zG``pRJUGDG}%F(e$`gw+2UZo>hPRXU23Z(`**nfqmJG^a())hepeGrUO za#1@9_NtRhE%bHZ&nSj}fydjy;KzNE@C$3fU{&^O5bUAoHz!%Cb8=4v26eT~x4(FW ze}A*<`gJv^Wb5!D^I){Hu~Fhamv|E8$8kv16#f^k&(CW#pXbSt-K}N%D@?4yoVKf2 z@J>fB83;)W&MwX~3%xWMybre&I`C$f{Ys;$DABhM4+rhBY(6B}9Jjy-34~P0tpw8{ zEwRdtImHb{)J;ng7F{62(eZW6lY7`O577Oh1&7N?lP&v-=5W37 zNN&grjh8)Vj5<|pQjr<{aOsFRsJVJjqoTS=nnR1f`n*b9jb(#!vp=9tBodzb=q_Sr zQLN1A!&renE$KftvBWzRffU-uu!2;rmPpw))V)J0=BwiQ7}i%4^QsQYP{U4!C5 zT$o#LhBU(8#(p}^h(P^QNH~@*ctrJor&Eqn*}NI`wz*lqvCkM{z*{1*)X?qaO|enq zfd%-tL~F?GqC%tX2RFAb5ZocbxmP95YyXX{uMBIe>Dr~G&{B#Aw-yavym)bk;O_3l zos?oF5TsDtDei6s3Pp-L!QI^*a?P_~bEzaANX6f{*#h&c&r7n7S#A}Y`1VAn7cOYx6YFM{>)D~8SdTpChi zF4dQ1Y(a-#+{RmEo;0N{XvzO{{?aO8;U3K;bF{g6_)gth9^Y_DEQ-rfWC^3}bt1ZU znV)JTE7@Vk;NcPZbFqf=pr;YiahumMHxa=Ul-Pi8PK*x&QdD}h_nSYO1^=I^>b@dAs3G3THT@g@ZtWS8R8r4z& zsbKhe9+vMZu2%Ub;n(62<$Z>J5unnC>hxsu<&f-{pR)2Jt<<#t6vGhg>Rd!J;zLr` z8mSgyud=f;@r9oxzdW6?E)uzna|znOt+L{E@}$5!47^IN#6q!fIHsMP$*ixbNa`?d zy4Gi`wq_pY`u|?_>sXAxsI?oqCp)0T3@TNKdeQf`64vKCDld zA>!7)#)r2}y_l+m2484>f0aQ!-fDS-l-!y*wx~8gh;LDF;C{1Tlp-U;RHGsZgmqw| z{mrjEw#6Btb8Gp?YYY3Bs?b=2kdZWA;z9gx*)v>9EnBoY75`Y#qPRDan|cZ~P8@>n ze^MBXQGVDt2Kioiel;-7s`=b|b;cYGT&eNBh8!$cc(%_N-u)8um6Sg7CiBPXjSMe| z%=qfFdhSmL58pGHtH#E9Hqkp^a8xvT3q*6*#T8!8?L9Li2>~@PT<=9?3;XfFfZ2am zPdEKx6m01v9}>S8-2%MAXTL*F&*D#CP_%-dQX56?*NBIfz@;qRBzS?>D~bhmbR-;a z=86C@f+(%r^WwCiH}e6kxu&)x*@bZem?N&iQW}=#hBB?ryNQ90WUTFeoDpQSOzV5E zB%_`pD+dTSjO1JO`QN3sz0-VWtU>_lz87MboHj#QYP8Vjt3uQXK)zYVzgGh@^v=si z;Y%De$EDa?=2@5+ox~9aQ^8_ydMEJi0}kPjXb)Qhsdp_^Awrtw)(?1ri}@$<&L~n- z7zexfdqlmiJ5JUs!1eL1Cqu!6xAvz^`^2yj`n)uLC9mC!>t}){x9XR5kz7zxtf=eF z>I;kHfw)-9(H|thZww5F?7e@%K}JS~UxrJHrP{`9tC=9}U0RPjue!W!3JPlztv#lO zyz0Co+6k_fBNCLsC^tAKrlc++aAfs%lF2D?Grie+`?Qs*!CtUUD=)%gr1-&mWB*Q6 zrAqetj3Xz&=d=meALE0~ZRp8mJ+ghdhmMzX2;H^gYhmx)uI+v%@6dwu$B)D>DGw)c zoJ3tjy)R3MXf|Yj3iUn&7Q}dLu6aY*6M3Rf)=(3qxM)QKMKO@n(~o3;R|&pfTLl*x zsoe?h(5(quW|#M-S2aPtxXtdr#Qbl$4u)UjbW<5BD5z&T`38_qoLdNF0raB={~Kaj!oaIjHauK8ZUAsk!s(*kKlLGo7t|W zjET9|z73gSk-gWE$!&WlyXr&I2ZL=x>;9Wm0TyQO&0U-_8s~$j+S8{FKmwkp7f;KC z*H`%Lt%9vyGuI8o1D=P=_pm@CkArT>^<`_T^8TO&CFLUCUA6ZTs+1XA_6u04QF%WS z>hb%mo)*s5K_LwiS?K~H5?$919;fB)U-KN3>8GPER$aXL#Sj>_J6Np=v{BN=N${hvt>0Y+H7<-eHx#*0NptW%EC zN($7T7Cz8RWC`8w9}kP|{%O7Ss}Qb%Jr(ik?;Omwx;95>`#h`<;v@ng+FgFSndr`#Z#OOJx;UNZ2sEE>e(we!kJ zCKg3|z~f*&(I6*>YQb$|gsfeW>Yr?D0(F<@{sf6|_a70C01t;2$-%3*2u5DS{#OwN znw+n$TJT{73K52^uO8`S{v0}G8xn9oKVrEoUa0nN@l|jBW3qoMD7PrMQ1<|8ieEHm zG}|lahbqq{8556bwFw5&7sYMjdLG8t$xr-QQrx4XFQPFmPb~e`0?0tXlRXxGYfe#((q}1b$w521fGLFogzy=F6hj(>sQeSurf^RQ;T>R%PG^0lYJU6 zhoIQg1{m-XOiN{b_wi#@dzW~!=>IhriYyo`@!Bu;>ky+5ZEE+oaBZ@{mqqMAi;LUM z?-_y-M}%nF+SRy4g9b(MxXKp3Z)PJg#eOzfRT3uV#YglV0MrCg#P?P^@Lu)55htk8 zaX}j>`KG*Jo}ur!>s`T|&>l&jIN*K5US=x1s27(dApMrg@h!y%5nt615 zo)8@-ky+<+L@&1JP30WIYz$RExkuY@Ay(El#3sXxppV(RjwY23HXMX(>a7Oyb&)E^ z?6O%TE}H2t7V7sdc$s2RMfF1&+^!iLGW!5$I!hcQ{nG5`UP)Rg03Es^V;ypyHdM{} zmjmkfk2>_Uzyu$cvG5KB5nh6jiHj-KJB37)_XA5$7l@Q1Yw(r-}(=|#Vc zeDm*o(1v&;=mL;5spuPCY6nfE9gxs)?48jo>iIk7VS`=67+3_Qy-psi@_l zl;vi#MZv`!ew+-qy1rv@WAJw3FDz_Hyd>2|C*T{CII>vO5gGYDq8SByupb*pO0x ze;79}JKu)Kxua1PBL?IFD9pHCf8R1W&ali_ASnT8Pc~z@?_l$@0wktd$M&Xpl}xjU-GIhk9z({hC(F^LV*h+ zcy}K$k7Gi-CNa~k8?=HmTW0O|-VYPsXeEbu%(6B;b*NZuF>pvJj@)#$ppO_6yTH)zNuxtAh&sNpX^?ijlLmJ8qhB zA>aM9saKjxdT=BW&m5NSJi$^)+r^cQ{6tf^5s=4ro#$A-T?3JNV25Y80sfmSyTR9Y zq242B*XT~szN60R_tCiSOl$V>9{f+pAvc!oa_7FZf(Z|fysoQ`QMg#D%;Nm6VC|IF z8D*RL)m^CLV8eTK-Gi;(Js4rbRm9P|R|n+pO_vIV4WSBuKD6dBf9W$Lg+sBNsQv^~ zUFCf~8qxYgqGeQvc;QA-2q7vv$N2%IE>l>r7kKG(CHr4e_h7P5AI?|SUZMTw-+-kyD5V9&w~Qu2!#K!$$y;g)MJtO8XiZJ1Jl=M{y_|B9^c!)sdHs;5Ui7?;-f!#f2oAuqjJ7*7;qzRwb)nj zXQ^ktfmOLTp6)+AA{VswoD+}>H7bIWu8z&Tx_(f$>oeO*7x3ghO9{lls$Qrkla$qe zSoWrH_~3R!F3@?s-hCbk8^$qkx2Q1{b0=}x~=Da)?xsG(o zw>QrM>Ub!E4KI`r?y6AWdLK@}_lQTev9JT!i^=sndjC(ZD>?}F>Hxkq0~F0YpD<~;#MZpNK1v}A5{>IoyCqL&}?^@}Aj z$&-Z>0+>o~SRy|Go)*ui+KAL(kWm4}#!^-|gFa5-DUG}D%x8U|>WG|Loju}}%{Pt* z)tX~2XcuZO^S@_zjH!lwtqRLbcv*$&6p$4FUXNxELPd=>NIX$^-N%p*!)-17zAg|?I5KipBG>)q++ zG*Y0j(Tu99@hl3%pu|J)kEa-6SRY07&dP*Hwsslohek`o!OwvK{!*8zg5$(OCHg?! z4_})l{M(OD=SxJSu>G+^LI9$hVq>!SHQP(~1@zL4h-xw7DA_Y0!TT?Fzr(W~y@1k{ zM;UjBc-;JGjHsjaDc4UMD@mAFmID1B_$A)lZucFh-o;}wjp@$XL*S+@#Vq02bq84{rvyS>_d1k-E5J?po%I#|n4FYlDa9@^Uq z-jzw(&6++ovjw2raiKB2;a(^nCKiS;6TWmFYjS}-!bUE>b~UnC+~D*pUJHo z9sj7}l1zZM59bUWYlg>w)Ai5WRR$}hlplIecZc#Osed(kJG!>3OM%s7Rrd%XEPCE- zHGn4WtK`vdo0Ef9a;~DsaE5z-MJfQxJX(R@E-wKgq)gYl>`67-+Ui$Eg|1JK?hoHRc-D;&oQB*w_^0C@8OIx8;&295lIHjo8zY zLfOS)SKr@$(@p0V9WEu?_;3i(^|z>vf0a`QS-O{bbW?jVZCgbo?^J>g#+gv39&Yu| zY|Zt&`FvbG#TNA@3jo;?iui+`%cppOFCIBo%+ykwX*x!@?BndApO|s)I^J+R*uUal-!H}g@^?R* zN0{x9fPm?@T_bdZ3(tzXO~_NxHY5lt^}72^b|b%R`|+eZ6QSWSKzSJnRCboRmMP%T zQ+bTf2_f(lSkt&>e0~|>(@l33J4+#5KU1ypyC%S4b<}3Xx-cxRH+`gui&13 zc^1}jN{$(Dz9;eAk3n(SL+xk!mwAa%gv_ReFUEQW5yRAmu}l^f$M@^^as)I z{mkUE$km9%K|7vG+`i%2>$;vh*H3TCD(PK#b2%X7FM)Be@i>9;TJk(21isJz$!ND8 z%l!%~PvCWvqaMXiC5k5vSeqV2$rOyB#SJ4l^M32VOZg|0mB&G=pVi$WeZ_J+`1XRr z_HKfTxIFl$db@H5y!^8zbnQblD&Z z(#%+#5M#R|l0H@RVBQ{Q;KM&uJyA*r+z{q#t)rj@Q{|dXEU&d|(_1UAZueWR1Qh;& z7R+L!zmZr&l?_g0=9j>H-0r(o#Lp9t1rVtE8dBQp?SB{Xb9Q-b=f$w)_?1Uq()o!v z543^)N&h~oDakw z|K+JFA1b*^(miGIFv9th8<|ZyrPW!i%0%y!c^pT{#d`XiFtQe9t9GPt^EMb_V`23p z4u_i!aQW})l0ysdhb^j#%wj90Z58B>`eC)JZz6ppf~NwQ&v5V%s63 zC*0Jy2b*$>pu=H?osnNPt`5Rh$u}IzW(}x76pY^PelihCgB3#5jks-jW-F1YP8EaQ z_$hv;D0O#7t9dv$I18T*W9?iAf^+tB$SzyrKlzJQ`9X8+@HSIYlW>n{v#wA>PQBFN z!e;kv@=jBd1q7!p37#5_AiqpOp_D_snGpKXMd^<$fJ*@gS-sl&@)qxFyV<4GiG#2v z_^-|Wc0%;JX_$`N4n+aAdJJz{^(+Fsuq|0MXw3PVB1jy8OF{Ql!Z1@PE@Vu*^td*d zD*05ss&o!v_v!A-OUS(~dpu}E!{u6Xhy)#cPQK_K@q1h^F}M7*n*i>H`%)NjqA0Ii zD!%N@tKBNoEES`UDJ5WmmEM|`RJ$c)j*HojsC}%lC4B41<2eYY?E*Vn$Ja&IFXa+I z9^kZ(!BPMyK4kjlYtHD4Y#C37gO*MR3~(;1SMMN@A}OtDjBI3z$Mp_CU%TRFJ77DJ zQ7)7d0oYAkauHV>fzCCKz{lykl5ivspSz{KPQ3mb$cjkSSSqiEy* zZr^fEAba2~rk&om-u_b_&6X97(UMjceXd01=a}4xp9WZGwYxke% zdg&Nk$kQn60X45{fE++f<$&w8kw6{`EwTrCL$If-l+(tBf4TZP8&uCM+{lDG5t5=+z>I+eGYWPg~q#yS{8GmT9_Iwh84wU<2k=8ku$p$A;WB(dDIj5w5UVSRa;{>V?Resz+^)r$o2 z1h|r%XDw{sdTKe2-+H#$t+$@SwTGtUVoHGn!KSnLW+7BnQdNm)c7R=drglnF8-w2C zG_#@V;#w({0!Tfc;v;TL*?2jzfo|QI?j54T^fCQ%@+0hQ2ISLUd0`0`B@{uB9qWYElSY|K>xtl zRku0Wgs)Vyi#-lEq6a^XC`4i`4sT?t?uQht~4QQ zsB2$`E#4>wa7qDrc7R$BZ10{T{BucrWpyPv+V>c$h`WH4-)VV&=lFYNf-@uG+o44J zZD|!$d@L{Td9M+LDF=3D+z5p!TSd0)q7qI41i0l9L{}qmc25GcWf~})f2ww{NgTNq zXE{1=?cl>kml*Knsv9uNsE)s!YWxRem*myYxcYzCY+*^Mg;tC_hQ3)NHl0D?XAaUtp0CynSs?>#I+ zf)FlFEnvzzP+YtAnD2~|ew+K8PC!a|$G+9Ldh26SFlC!Q`BF+j-B0L~#=vp@BYOH; z_0_T+a$WBbzmG511BwXV*2W%O?=bqyo3vc(sd~=~_*H2~ZYg~Gi4UV^4riOCVgK?F zeElFXm;^UFHZOUnq)T7B!->GEB?b{X>rcmjM!KGTfD6{Hv~#8*G$5LWpTUX=BM+X> zkDz!=PfqSDyv#wu_A{)^S*u&!B=VTUwUApmCg+H&<*1G?Pk03_fhnD0&!tic&@HoT z?3BujR=hcw=RKqCD_!|aeiQZ*z&)#$Vfad(75ecS*pL)g0Dho@%&?07V;<%*x0TLn z^(Py=D9wHaW%+i>PrEVGeXb}D5X6)S(feN9xrw3+wYA&l%nz>yxw|=SSb4mJ8n*!OS z6Xq<-Y;LDImm$KI=DMQ8^Kp@P>-*uvmuL?|%q65U$?Ef9+qA!QB7DE0E10)tCF%aP zNaPldZ)TzE_RX)^P$Z;Da3n5-p!P_7YcLYBiwMl9++JvfEtNP`9Gi48l~^N^jT&mT zkIeD?hK@Mwygm4ZN8}fh*);Op7J!J15PL{86Cj%7*JUfB?}6cQ?h#_lqlIO$3?7xc zUzA%7e(r#u>)G@SC5wBtRJ++b7chN5;u zu-wn5^`TtSms-&tN2VJ~AEo9CEqq-K?JRA|2RXfV?C>*CCIJM8zR+9gS(^Y#;>)g^ zJ7cL_Crg$hH%zQyDd&6s`*@wY^)MbhKLhEzSp;=kk^HA<%c58kb`*zYwFnNWFl#!6 z1shLTmLP!G-Yd@hjG@mazg66v;~Lxw&mW%u+K!NvmhE+t!z!Ec8Cb8wH@0YYVIi5eWQOKKdj7&sDGg=OO?$ z7~TR96ZtEuAA$d2ynlQKA(E&56`5z*h-|BWCFc46XV}-Ty6@!8?knAEPo){x=d>*K zuDAZ_?W71p>q^>SfTsJ!bX%Z--ydx-Y0p;8p`<;7EbSPbuKo4I~Nc#ZF}2fq*|G3gDQ7D(dl139HoUOz(+(tFvah=0g#Y1P=ZbUv$G9GQGeiCIUF50I@mLpFL-ydAQs@R z!I5&c(bOGX(&rw>YAMI@e0CZbSXFg^AEdgREptq1(BdPse>7p0n9%q^9c(EIO!@43 z(Hy2d_=WcAV2?dt7mrK8NOL?S(b>S$y$zhpm}0`DQZ6g#!MJ(w7_AB1WBcDp2nA9e z@zW>riDX9%T7Q@Mg?_+~9^uua<7nE|T#yoylU}=B)R6KlVfk)`h`40`vk+SqQ3#W` z7k7#dHiT|vf$7Ql@^$x~!svtFtSn6&PpJ;)093_*I?w)SuZ6Mo9-M#M#ktMPH zmIo$pJ4rl8>6S?Qc3+elR$C`7O5&a!@JJRutJP!`5S~E%PWNC_1#B+j6&USZ9O0Dz zFsIx>Pq&`pHMHn{Ivj*6xU)Dh*mVOW?ls1?Xxm|Vv?>>JZFqs-UsHm%Pqi?EI`lEa z^9n=H=fmX@x?=uuJ_{zHZc?I!P>73@E0&!t`jA3Q4sfWCaO;+w&~cd!8tacBW;?~i zIlJ#NBoDpMgL9Uwc3mris4Ta;^(2q02H0FDaLa{zo_qovJmX_T4Q6$(lqCr(hCQo2 z9X^t#e8z3$lO6uS2#}NBb!T(*EBTdNqO>VU_lAM>r=N=&HlZ6Gk~%pyxVr#B&&=^6YwN}Yu2(?}O=_ZCtnQFkPv zYU_cvw7<|NBQK|b4WWV@&Df7Rr>&3wec%6HeMXT=mx5- zBymavOpctnmg|qO_yw3HV9gIQ+r8eLGkdtYPYf56eX@}Ih=)v&68Bb8B_*+9JnP#$ zuj5vGPY^4c7!&a;&TkXLgx_{3Ayz8h6ei7}NjP)s(IHv$Tde!-zRi8Ft)(r)#*i}) zUw3V_{{DO|peo>XUhYLR;Y7@m{KThl?fCSo?Opo^wXY&2M{~SLH|j{)+LXrk&BFs0 z0oOIi1Eo7$^TX?>vsOhodd7P=Q;Yz~z%cf7FV8B4DCpV-i_B-}&$$v#6}q-%PHLBAwAhml*u3SzBxBU(IH`U&Cu)lA|NJy5TiD&0zp!@S%8>0&xxs-;I8dTUX* z$EHyH+U>3!Q~%q?TU;-A6cs+b!cMUJ{;)|+jWgc14GS(cJMzSKxQtD^paAPfYaQKp zr0VPg8*$&Hw5@f&03>ZJ+um3ttHV@;B66PZD91d9SnYbUy#t{0pxyqCHl<9sV7RKj zC9L9#!mQ0Mwf703(#QsF%D4M^X9&dB`p0r*Q}5Ow)J)3152qp4+=KpB#jEc?dR$Z64^{Ysd+ypRbl zI70wUJ0@el-h27bS#q!;R3m+9t=G&ZWo4oVd*(ZOp|d2Imz_Mp`s8146i|9z_<+Lb z(fvZenI@GoYc<}#iYH#w=X{gsJw95{H5xMQ4L*;Rh42-;MeVn)N^(c6y;!&iH7=}s zklAyb`duLlBD$?Ohl|rFv|6DUT$mxL^6Y|MCh*BhrLjRB_QhJL~!5Bd@JbELK~8fAiLdy5}c1S zxlQMt8~yHo{xXrmwoTHwz_gCJg_2uB3;Nv6&)#q=8>I{W+`C|^)P{T{cNgznw1RKZ z>2d=S5tYJe6x6&2*lHoB^cfE@`bJEH3S&I6c*oyyJik5P=``O|T1dPbPwBU$YZc`% zPk5eZpJ2PzU%zg!OXs&Ox&4*algZ}W&pLZ%(Sj@nSd2>I_8m{ooAFGxHkgz3S^3Tg zJNbbDOY@a>WT?dy;kI!Va4^fC^W=pt*45p^kB=(J4NN2TsdRuA307&B_3ou7-1vBw z`JQA^n{011RX|?yEl0dX{=fohVNSl%k#$&dN3F*X72$~D=PQ-^giBqlt@6n;&uITx zn<}^FLf3Te7WL==@-Qjg_e^M#)&0|g4_M}p1M4{U{NY~IY=GC&DvgQz^(bg7SXu$! zsvC_M2=ze-{pa)E^S=jXviN~_?S7NA*2KE*%w4J2%lAWs3jeo~|qeGX3Nsa5ErLg>_^>p0C2Y^9|WE$?0q7u?qPc|J>@`;>hCuJiQ z%95^3<%XQP++zY!3LUV6_QnjKvyW>iTz^zWmjTSjx1nJCBu+oT(X_v~wXZC7fBSoSACh%f2D76m;n6a255 zIQ9Hhx6c$Yr8vX{?P4A(52o~vqp+KcQhPx-7C^pMq z=+$!D?TVJCmH3z<~RF?Wl+xpY@ynb3mX>7)b8ujVSFo`b#Q^Njjk_KoJ;(&=Je z7qBY1xUYkAr*}PBc^OY=IgBvc$?%3+DyJ@a^LNnx_2{{rW9AL3ZvHuQ_1k{GF+J2l zU2r{8qS8nOZF$5eD^f0mebNEztA40NOKID?ShPtk|m8Yk^p5zPd zmWyp<>8Q_;J{Kd2Rke*yw%v=mk*!VUMLo~0ql%dFmY^euV5hk;BUQ2oIT@~A$BH>0 zoLhM{@`AJ`_R^syhpqSsj&kv6?35Yi=TtTlRZ4il)oXK_uJ%aFi4VS$5cGt`#fz6jqiC z6;UOIwQuOA&Bes#FTX`2Bc?NM^m0!Y0>n1w}5h0vBE!j>AN%%9}j zQZBarxa5zMGC~e@1$ybDXwt09eT{Y11g1VD5{dYERTe!q6v^;vTDrYoLgJ~zGC07c ztDTbVkmK;!s5elP)fVHuss1*H(CRgivR_TU+>^F223Z1 z_KRai0*$@h*bLk0OsQPAHk-^abss${G9dm+J)&L>w>H!4rmp+;i?oS{506Uf+k>nX4;z zCk2hlor6u`PjfR(cr6~~zGjG#Z4+(rAd2Q~xly?qUpeVtVK~jzSyUxr_Ba|gh@_1a zwKK^M8c2kpwI?5r;&%0t9|p0!S!T1b%!DQ|^V?VR$Z7A5Y7@Tg zX!eU|NH;%0m<2~1nLjjhmAh`=-J=A**xqs9iTpGUOHRfckZV5SID(d(KPn0)*UY_t z3t*_W&)|*B(in`Ts(t5CQm8sq@T&MFqAzFWrzn2+Cov;&;i~UI7!8HMcb6;SrgF|E zMUvW83N{gLP-P96JFxk(9g^Z>FilKO(u<3ElJM8D|B_2f4F7E(BK zug2narjfRE$c&Jzt%>H~$w~jp=XyWs(1MC}!mdecOZD0YkJk)G`pq@Bm!Ol7D;tB` zJ%iS1`SB`rp*Pr&SuSO32V|wgNnCurl!8B?M3r1?GL)j@=+5hCF9Y1;T@rL6>e2*w zy-Hv4v+_6RTo2j{0lC$=VL%O<*29wh=ST#iE8yItmeUw&%S;$ZzY0XTUr93Aj6R%G zPgA11^4t(a%4xd*)>WlTMdtN7OQY9CEP=%6^OW{upG<%9jl(?beF4`MTRj+zJb*5Y zdBnn}pn2r8rB`Q3UoTbL?IoZn!1&MoF44OuRmud$>>m;{MA`Jb>&p^fB`#bfJaNJ@ znj+kOg;{CgO-nX@AHYJlTCU6sQg`93%v>Yiv^umuO7#@%`@j_ z(%*}F8de-^FIhfZwq2bSid0kneB0%8Iuvho&u~L9S@lK%uy+}1h%QfE54qvHExJ2g zY!CISjATe0Pi$!oh=QzZ93>m&SRnMsU5vp_1HxlLU`Olek&UIPUwovQNo@#0WVSD; z9z6k7eKM0q)Pt(;;CEt%-$zGA2{zGbT*%fWka-%miVo&80+pgQW;T1v=n05BPX1hO zdf1-JL0TI~Ut%l{m(SlX+z)N*mx_IB!Q4+G>4g8mb*hML&3Cb7t#g2$7sVOlwbo-s z1KxkobfGtW?%jXj^m!&ClaJ_B2vzHP;}hZ;)P65%?G>nRVXu?M{d+o-QkWH@p~X}L zd~eYAo^;m0&CSo$>Pt4Ns2c6-rbT*8IF|WMlHF;xe~d!y$xpWondcg)AkQRe2h zep80t%|j0}<>ckgVMsVExhPY=Eh-|bp^4BjTWfI1kbD|&KTea@h$Wr=Kj;sSEwAMLO2L;;?q3ZVkF*xyu>R)QW^zsm8ib(`!`3kQ(jd!Ac?fb`G$Qu zpXz3fk|L_;CCx(^HhS2JbD51!L^Zy4ihluj>7o#2sLIqYA@E`m=G;kq^^cPMRxiT>v1d>qOwN@^S zLV#kS zd`K8eBPsV}zo*sbJckL8JnUMY3m%Dj?9p6#6xb_;;L*8t-l=U@&dvfi`UT-q>DETq zy_+2aHS5oV6Hy}q>7ZSq(q#8R1gf4$H&)?**A^xoWr zUN<5pGfKqE!0-M9sfd9ZVrj0U9OLnY;KE63Nt_`gv%Hq0M@&z4$4I7g(U5#;(m1Bf zcceYG{?GF0K+kktte=f#6Nsxuiu*ERr2ETK8acDsdZwe4t5rUO5k1ONuc4L(kOXtw zXjUOQul@OnptCHT%0>)<1^R?Os`$!>tj(J_XFx~!$$U8TOYw2@wuI0T9=^rQ00+Km zBg9rG@}Z}95LUq(4e1D8gIFA0eoD?kEWYLXN!Qrv7OZ|NoL5V}g+>85H-F8s5Hth% zfbOBVSUCg!~01#i~P*LD>fvpB+dSguLW{YALc}GZCQn`#2X6fO60#P$_)+A2_Oz7G-C1%((_>J{t*g7@Ni?IB+Mg zd^yy+Ii!~G`kjAqQ3N>iFp+ZqM`HU&3m`#;+Be6=*iO<=W-fBhnoMZ%O2vTMEZ5vA?~gH(j`yT3Y&oia!lhy^SfB;{Fts#1_@$yc?s|}7 zD!0tW&f3lpsH(*?VYXX_bhRYThC>O$qA7p^oZ{;c*tu;kj8KQ~pVf0I7S8#DYcG1A z{_O$aT|0l~khWL_Bive|`wW@t`FADe9zVz0HZX)tul zq6`N`DvCb#LGujAFVMP$=QE0@61l}x(j(C%r-=MUH?F34W06C{Y`&^7bYn2NrLZ>s z;bOieO1D}WuUXkS2w%^Us`#L34*}_aH>%PrI17D;uGm}3tcHTH&3tjUmZ`FCN4QO0 z4Q0j2yxA&SS5NVzLvwlkNP*VP8+H#NLD=ZFrVCN6Z*%&cdh(Z90l53Oa!MoOc&9yg zHQzv!T9N~dNFkt7ecm^kJZCS8O#d#SVbF=t!e4l8!6^md?Vl?#0;XItBO&z{g>pl# zI8jaIj$2gxTs8aMO&eLsA(ENnVdt`?D(1ZP?Vt&dkCu|8u9KLr%CunslEG!o^;zVdF`Ms$66+*g{;qJtK70aVtYGavZ{4*`OZiiJw zL+~QD;ZuiThTuT@7%?yZ0M)F*!LVT7lrVVUkN$sAHCZ#w zG}CoW*NmY&_hvc9@gui;xPFxSNJQ%+?H?q)dMIbYbFF%ELWR^5C`*bz2CA7&@k{p7 zJ7IShX#wW1Hit5~7onorGIlr*YNo3t+q_s;oQ~M@f0x?^cTjJj2iNU&iVFrY*t^_` zI*4GdS(;p$k;zI@raKvZ{H>m>*SOt+iW$4~?vW2qjnqFJ0+nAv+O#?k=Pt&b9J{=V zWr66mCII47Qn(e6u-}<#{fouI^riYUBE^G&`m)$*4Ds(!LD6FEJ0>KU1G?%YW%&b* zg>$&feF@1dcr9|f^Fl{Uvhavj%{nh0v zn4=|wcQ$(+J28n{0oKkkcj=cOQl|2|25OEc?KIErep|_7nTEWMiyMk*tj}sbHG9;K z@b{ynSom2otFC);{|EC%lnT1JoY_P8tDRwR;CBOZA>|PUmCNE^C_c0s(S7Y&u{byY zzPq@w=90ANAgT?T83gHnqK>{9$O5@J(4>FQnsw80+14}^J$>okr-C1#Q21=he1AzO zT>Hq#Z(GyxXhMrmM4IbR;`jpxO8ecK&t4yLIN|8~m{o0(-=%+%yNTp&lec6A$4TBd z#HLIG7+M@O)lDe+((lpx^C$SeN1P^d(rE%!o(Pqtb|dl@9gXu_vYU`-Ey1duGcm#n zEwXJ((I#Gmh-2jAhP;1zocL0b$ZEB*T6b|-WL+LB;H=MtF4?M4I|yHGp~QBtdV4(` zw}$T858$?0e6K$H2k!?2LBx^#h5vGm57P5qE>0kC!wL}B4dQhuEQ)dkkl#GDuvZcC zI=^48b-<}tNSsARC@`lJ$`m|wmxQb$>2TfV4927PUcJFOeV7g`eY@oR*I`GPW5P~H}i^g4d zCdF_>D6A%biYIcn&fMo5Y+!ViaJ^AF;E7ezPkcIoZaFvgCP-zyBY8QJMHy`+Ok17* zbh36jgbwbKNkTQnnUNJFptD>}_{l6|Bm&Vk%Yu=jN(1yl;C@?8caN=ICN8;KxkH21 zPj3~!P*Ui(hh1gFF9@F+)GCw=jaj{GHPC-8{JEidJUj{@%6vaFk)I zn=6&AdgtP^O`#v|zd;t%${;Xv|Lc!-tdu;$0mtP*_ps9W#Go8YsIwK(S!lq>J|QGD z1EFzpPn}yP^i+S|wMo(>o2j6OX_r|KxDhS+Zrw&?XQ0_MRxrj8FKg~%G&t- zIjH~}f|_WrS~S80PE!s+NtGtGfn*C{rhI&uJ9<1(3BCj0VJ>0hG6GWE>A3kAP|%@P zoFFT7t_VEqtBSkihi)uEEjsYG(;2?(pfsTmb(tFo>%PZ~?Ca_W* zriJ%}O>>x%l7#ss6GoYAmIBlB+{#cK!$Fvgoh8;apcbYn5 z^zr!ERdBR3!LE3_tX2AQ`HM}rk&k4vnUIXQq5VcDK5~M(>Cy{nwyJQ9=sh4=ja1vIkGz38404`DS{=wwEiGe3}sINokW zUqB)X|K5#Sg0_hST66q0s#8hB{%?UxmnUwoCQSY%Q=XmrT*0uaF-s}-*Yw5xMgKE( zwg1|pGqL|`BmT!g{gK>E7G1S_h(d&=zSSQpQu2hngrGkPl|O@ibpDidtGqZmcaPut zW#WGJ%1QWX?v;Dbg^~xVX>!fQu+oxcDN_lLy#@RBd*lqq*V_%@^5WpfG3oO;qwS0mboLN0w)4HA`>xflrv1j7uW8A> z$W*|kZM)`>NjHSHNbaiN$kFxlI*C-i%;#?$+Cz5?(XjH<{J}TF4y}WdmqRC zer$jEN9SaWhcTY#j`O;1|JZYC?rBW_?sti%FR#vjR%nhTsK@=&PIXS+`PENVnPhJ_ z8apcSM>1d9ueXm#YA{!irWJ~{%4p<}1$bJKjouow#wmxyfpedi^f>yVIY;33tg|7=D>rEDc|SRIA6mISjJrcNDviG3lQX zFu%Mw*fDOZc~vR9yxS7m8E2cZ1~S$($#&HWFC`ocysF!nI)!NN-oyU6srN)64Yf8b zfg-rt+K~Cl@~2f8Prkh*B9J?p?iwNeybZJ0sa+IY#rly&zne zBF=6jpgBnB_|p68#yUfRIEV&^(yq4Agg|1)j2JG6)jfSSqS`euS3iX?tskEGbI$QP zh2v&U6*y>T^b)WkK=!A>KVy16i2I9@S4)(hNTYRKMtz@ygs0B(5){jLI)ZLX4V0f zUqS7@i+iRsu^mC?7n{gtAlVu8{5;s8jOmnwpfnQU22mFlhI;i-&8M87q7X}33=D~} z=|i4cGxEN;tdGnzrrz2{>0w2=Xi{V#?5CM{dKX;U$)3eNKn5jx{*XaMK(#7eJHiCG z4YV%;^S!_HKYs(32k;V~y$nEiW1H%7x{E-7{3$;uNQif7=i~E|;sSTiH~D-X5(3w> zDcKkv!5?Sm*f=eJ9MHg8kY!Bx>VZK6`Cji*KZP(v_JJyb(5YPPk|#%jE~GdHIzuyw zk$mFs>v@v70WHv!2hrm z2KiK#q|A^v4knY)o}Zy(-SP6M`%lf}Yn}@xlxzMZ()=M&xhN8cZWZtvIY6?+#5Reo zL|^>_eZjV4Mu;}Oav_X)DnebjANGzq<^)~r#4yRR!t>?Mti(&r#uP6<0k@#d5BtW0 z)V8 zRaIm&O3=G^z%;qd9mdj}leFOrIRn@^2EG~ySt;)C7#d<$CA2khN@iCV=iWx1=|=+H z9L5#cMCYtEP~XWWSMg_nb9#^Z=a=`s5vWzJNC?||v^3p){iG+9W=PQD^KyJYWtWPu zQpy>Xk~{*M!Q?XA!H3GErT-!L;tI$$izr9-jmqGbYwW!<6DKYR`$3uxEZy%2$L?jL zOL(IE&6*jy*ZK?+tVic!+}XU;MN8@TfGoKlO~TkVo_>-Kx6+m{!Y4z1TdPR9F!m-2)y zS+Udyz@GNLKu(UgL}ek;nQ^oK(h#M6MAcW#rEY7l@F%<=BI{OScyRzrMNZ%Ohl!IB_ zEB5B5$+l!$k93@o7;Hp_66}JywW= zdbo4x0${VaFEN~d&E{vVGsh-5+shu8P&6g}26B^MjDe0&eX!m(j?E1tN@yuwx(H)b zgV8Ty?o0n`AZXx>BsA#JG9R#C;?UFR`=6iX3>Gj)jXHWU-`-@IYfP(v@C(H|h{RtL?5RsR9xDD!*#?Cw=^Sq zp!MFDKjAImYEQMpO7Lz&GQShHV_-_q|EhL0n$EqRrcyRVz53^FG93TG0}nd?CJUFc zz983*cLc_z5%#cEKD&tj>i7x`wwLI`&)5iEBPs<|_@|HF_dYcNaqlT-*DLgLpP;ph z-@nUOaVaZ4L(FHSP)}XUh{_hD>W4>B9x10ieQLG!?%+~qeQfvqWp>TS{Q%HUW|&9W z{lIfjrK{pcyieVfij19zV63Z#OPTNWj8ZubX<+Waz120AzO!-54za!`^Fvs&zNVug zp@u#B{99`X2!|o81ruAo-jXDHBv$NG@8-C9U`ZII&3i&MD|j2Id4F;9K0okl+~mxt z9r^wjQJi;-f>2@Od@temRcaAGq13lSS640c?8AH%uL-&QzJW1gevVB+W0UU#%=elU z^Tby&=d{_CJgjA&%nM-A3SUw=DQZUf)OhkrA*m7`n37FlFbJC=X!b76>^y^=iu1ie zQm)o4YomdDaaHPVCZVBb%Y9FiPjJ*RQ?Sxy>)A|*xF_(_88yc;md>EHZ9yLw ze}uhTMcC4rN13Zp@wEG?6iFMv;W#^2y57*v-)QD3{SkQhY&PVNXyZLkUfS0!Y0%>7 z;_xfNZ&v8aTGXHH4bHx#K3LIAh&47af2)NeiL-b@Ncle@=q+S6onMRg54};`T zd8f!zVqmF&6!IbR1&MAs|9r`j3W&vF81 zXJ0pjg?!@P#2doM%4B7$QmqoC?rpwv`c4~f;3>t@_|}|7i07 zkj*M&`$#qn`s>$}qA4Sk1xz`>U!IFU^I+3Dv2DI{6$ls58~wOHc>Id&n?aV6PselE zz%IDfQssb*N*Uvq-mYfb#yr=y$aE#21>X9w@!X;eu?O|OpX1^vVrs$nxa0dWc|owA_Jm$SG~k=#BzC~nJMuK zgqYMqWU41*knpqFyQvduV%)Eiv>vw82tqZ#0ppreptwM7ty#K4GoEHpCdC71x8ID1 zAXTC3Jo@&L`qd|)+lfFZ9K+qPh|f!0&c)d@^!S9^P+U}DDCJOE(8X&IE^qHTjU8pf^&z@qi>>5dI!orNtanJG4l_W*CUk%EqbRZW9 z;hUVh`O#8=7ab?Wxpv{xm$e8b6&dO1v~0hdlY_AY*Hu)IKi*ECWb}O3%dW5{cM>GE z7Q=ToM7lG1oY;5Ub0W81G*1i~XAs~6Kg*%8He1LBFy(vb6sl^U7WtoJEMD&vw7e*E zp*dEvmfjb?HO3_91Y;U2mAcmY97{i)E0UZ8s*=ghTQuI|>ra z=o+oP$1Q})Oq|GFb{>^=ulR)ES2Vhn6^H*Up$!;}TYESw)2RDzM@Bfz&~quzrJnp$ zuMfehXeqC7VQb(fJl2wr*GNeX zlDheWaI>I<^!7#Xjo-erbd-h`fZK0Zi&pFJJ2D&bGs~4O`|$b}TVzv>KG4}D{i~y} zIr_;{uE&>qH(d={)94A*h#}1g=KLmGID0ty>ph<`q8o5wLEP6F>`C_DZo~6b32#ir zp7kG;nmU{eP6w|q!aUJ@v;9a5cZ)n#^c&!AA~yB~*Zf20{Oz`|DwShSD4Y2`11i3q zd`!F^Jt9LEGivaXeAQFKFC$5xysv)-1YMY7L!8gIL&0_@G=<~YY0jgH z?k5$^vUAy^mE$df#C5zigwMt)&j1{9VfJqvaxfdgOS*+k&LCnwK%!BDq=nttrgPIl zbsB48M&T0KE0ymDNFznqw-b_F<-Y}KTg88JQY1;uO@0zupp}Q_>%Uqx6M328t=~ycgK+%lfp05fs z;S&rdT`%rwWT$N#&Rz{CGMEf_)=RDjD9jBJS*_lctnn=PQ+Ic$AjmRhiQ|EGIbSa1 zITTT*YLfC)CU`b<&rXZqp1yqarRK@Q!VQzhSnzgdk$^NP9gw6xgci~;KHYwTdqZYJ zRCv77$i*|OFy{C?PT5}ok`p4N3N%WWSs!Y5^Iu#*737%eBg(Q@8brW1(rB#By-%fq zbXvw=REV1hVPF_t!cwAm)G(#F;U<(TE^4sk%I8tp6+xc1F}g|$j`z>%8I zCxK&9x*^7E4DTYVrAevZ9A{@;(>PA<$AAWLu#s&=W;&u(p1?R95~j>)D_$}3%X=?S z%PApGn`E@?2wzlUcSx-3@%c`&e&(vb{K~*oTLMoiC$;7dZ|Ubi6|YRNe}#~Yn+)9z z1&l#Pe?DL3XYN%qu#xu4<0k|t;ASaZ-aWn}0&{sQ)8{yG-##|h*Ep~Awg9J-iknO* zBs{R1J#<-abqnObu4eP;>Eh{3L?@;ei!`N&Deb0EH{#~zYKaq~m^PeH7^@KT(#wc_ zj@hZZBABBEFjAPnxnz;Qrp+$HZEZz3+;J?sg{M^>m7Gu~MMis()knOR;Fl$AKxoLs z7&TKJp(K67qhTp8u+u+E4T91*{~xqvcN9qVrTMUM@y$$KsuKD|YCAww`VGrG0G0GH z@aHc|%9u){f#MV8Bwyt++xcztj5;1q!m>rjIh`R7+i5;`q*d_fh2R7@%|L&YFAm4Y zH(@QtAXLn2yAyWV1ygQ4&#I%UJ}W3ewAS1F(jg(ic4y>J!K0proic{cS9y!r&hj9A zl-X^K9g`+_OGTrpkzFmD)lsmZ!R>_IdvTT)ghH2)nb_WauW>gTP&(P7!g1N{4$C@x zL$OUNdJ7e+)WxLD?z@btHPhYuZpR^v?Y(*fjJG&R>b@upgBZm;cUNsh#Q93#)$F{k zO`8&BG@mUVw+8yAe%Ip^AlSkIf~_^>pvOpq>Ai7C!?yuA_c^=HqdM*!vnR6x zu2W9}06hoe{(xSfrIS{8LfuavmV3ZprR+C{$?|c9@35e(ez{i6h9M%MMORw~C2_6W z{p2(){K6jNpDv8v)vqp0AmJL60E1IrbPW-*)IoD;w~w3rVcE5uYTiLoKZjCdV-$S6`ibm6^&HI8He+VjlIj3`OMd^%Sz~}8bK&iwGKU3%lquHy7>G0Sc!A( z(+38>7Bcr)FPWYij&4uTatkxj>2y=K50PMv$u_5K%ETk=C+Mu7n{)Dl8M~P+hl6|e z@Tu>yCZRc(2Vd=%@>!S#SaUskR1W*9HJClJ-1w4pe5 zOa7)ugvar$bI|bdYhqxdU6p$Smf%?5?W~HlC`UA~yBGk1Yev*O9%9G14g0c>ni`g` zxt-D*9MYJOo^1tzPm2q8A8#>NJR!B2 z|0&Y{5c2yMSo97aeDFX!&Q?qj#=Ib(YF30mfjze$#YQ&FeC`5+?rN&t_Y4csKQf$7 zf5CNi&f%W-d@gTOFL)?-AZSJ;{1w^!#|dA|^NSL~eJ~6)j;8Wile!Dff7nAUQ*%p- zUh)G5?i%XgOy2c6Y+142pdx{QID$MOiRvRxp5AaVDGs^Dx(WrW4>HJD3MPs79W*5> zOo9C^K93GmEw}0mQmF)IrE-xdCerTtr%3E7nideOhz9tpG3qrW66WG` zTfWwWqm1e2OZ~tsk9gZF$Z96BrD0vTcx?~^Wy060+TJ@!{sO_kqS4KIck492Rw_wD zHPyi(=I$-|W>dHQxxrAaxpA(5muK+F1{9FQi^Z2m zHx7({qxF8c;wqYQ$Q3sOSL`6?!aE27`c8*{u!V`93c6ad*d|z7|_iQ{WHTLn(VP5+cUDUvg=TfP*@7JM!=i1N$bTe0zhi3oPr=*g^t5~PhQYz4K% z`)Hba6()bFDd6g=<;-{o-D$1ijlV%hzCfG4-WY@`Gk=lzUc+urwZhLlGpz}d0+(Nn zk%i6KDxLElE}sh)5Ol09-j3uoX0ZZvLe-h3x8y6wC>CD*9Ck*jG9<*B0?cbw>ZbNPto*#n2OL3%)XL1MS`og`);H~lg!D&Oy)x=9e zRd2VGla=kcHJmXJ&tL6~nI=0adZgvFk64s2n+15TlCHU~PUl7Ma0vGKQVbspyR!*V zROjjPO*-*GR9yF33HzQRLQ?~t=M&t_#GNtnLAg-<+reI}#_e9y=VCD2*CTCvpLQ&% zpZBTTkonjncmBod{c2_`EHC0FYDg%f%0wn7DAPF%7mmd9_L>0Vx`(7w1>>AzI94y$ zU@(7G7nOL9V6HRzp|`@n-Ai$0Ty&=_@5Ou1=DbL~4g+R{YxqqM0yQjYIr%8*^zywZ zUySsdz?;6dwxKK}is)we)X1)A)Ohs3`txrv=HMVWGdo07aeJ2ML9Bdr z(^9t--0NMZGfnHv`j^F+((PLW2a^uBZRO;h2K01E3n9nM(A@@4dr`n!S$3BDZ`RZ` zc9u4BY4J}1z1P%jH-AH@TMXiWCtfzk&voX)k8^J|Ge2o#okj93zmW`8*ytvuwF9H zLhRfC72k5g{x9Gz0~NNFtTkxVozjFF`;>ia|2whlHm+z{RwAYoRX0C+10BD7?zVW6 zvDP6;ti!Ki5j10lLdQOz}$qtm-n1`&8KL??o zu3Tm8ngZx474X#k$s<|a1+Rfbl3W8ubc@fUq+@n^gc`@J+{wzeI3Un%Ul&LpRncPJ zK6*4Nb^1yGX6%*o$o3wd2l-I>KoOE*fi4m_@E%WHcLXNoBIIGQcwe( z7yI|0wN8DTP7|`XKHM+8J+}VclHd>HGP&0U&VDW4RDI=Oh|w-ic-vzoK{+#@DbH zCN0xI!uFi4B)~qhv-kU)0RsLR@gm@d=RPXBac8n!W6VPcY=Nh=_ZZ}9$j1Hmilkg2Q%Jc>jMe(R=W@*$!w(2x&MQ9)!QN27%9bYGY4i?Qy z)S!Ib0+D(xnF7-vWNRY$)C&KST(#;jlrVp`40)U~L1@Wp+4%sMF+t!m4>sYj4>%2G zU8IIxj3cC4iQ8&yMM8hUWLD#+$O14Qsw}ceKyd)Z#&LRN|13^2(~Sg6AuuruoH2i_ zUP-wj*5X6XEuu106b?g`O%7(J!#%2Z#U^WHufE}-L^x4uI8V?aG%S4m1)>dkmx3tpdjQ*3qU;xd zumSxT^@_qo!U_FlxuhZTF|n$&n_FHLQP6!Z9E=J5So=0`I2VUZQ{14vJG~aVv}0&r zHjT8QjCi9DEugf$VD{Wz=QpP1J1VWgOQiAEn{DK8JPckeSNS9eU+(i$Ep;hQhqs4M zWWU#7hCisV+N7W#+Hf@OQI4w-k5de{=MoNlYcBcPMbYIe*wHuf>l%$K%F5eC{O?r!U91^ciFS2W2;hEMHMQ7(Swe)yzunRN%eYvYhe ztX^RuUSfTJdD_jNF1TQiXb91D4QN5zA_jZ;so2loK1i(yQ$#4@pZ>zRIQkxNt}`=n z?Cw7zFd42od*|aOTw25e5fLZ5)uZ9cgV%MxI)$`)kwCaEStyZ?G@ilj?7_X)TRNjF zhM!OB!2soI|M?b&E_LP1Z$DK})&1KT+08nA|LFccAzYY=2|O6(M|2wx09$?Pe1NS6 z)qJQY69k0$d+OW+((T=X#&W1ZLAioJC?Nn8KmM!e0x_Bdus0{e|JI+O^8C`DQNxZ> z&yghQDVB;~Qv7azTJJoxKiQ~<-%*2rov?O%%4w>rCI5*I{=LtVnvqkxU6d-i5%6-4 zN<{#c0Kg~`h8m#hRGdT1i#Y_vi|h(&sxRQ&HZHK_Lm0LW5(96{*JS0k9%*0syO`Qv-z*72xL#v=&R7 z{w)>r*)s;3S)MOG`Z5w^a?U188V(-6jm>hXl3Nqk^LBd^;kC9ZmgH5Ao_FVz1_oMk zN&rU;?0?|owRh|Tbe+t>`y5T{>hlJ#?R3%Z2d<#&hdupqM_Q{@l=?J!iTnH;D)B4y z0x43{92FH}^?=r$#%ma0*2+I!uz;YnLwc=;=Gf(?w~f2W4pVv{P7(aNO!QzSv6q*l z-81Pm|IGfTsQ`uhdKm!#kRb==U0B}&_y4BAYrjknO2w;6*f}KX`^RMFQ;ut)C&NZ9 zL7v<2-~;6RBHWMI%A8}Zlb+)_O2efj9Kdk(4`id~g}-HD3t>ASuE*Ont3%v+Kf z894aj4t%zQ0`yx)Y$Cs2J;|5JM}XMyb&p3Yx_bC|iH+2qAlh`UUjBUIA4>!2N?urJ z6Q)d%t-WHSzM4~%nN2{29bq;WBcYqJ)v8$@dU`YQL&>ghA03%ujMcAI6A!D}aXZJv z4m@Aqsj0W42)CuIQ4@HDnE*b&`69C~S}18s*v07AE}0QuFGU3^NQa?b#yHw?cH5$_ z2l$RJ{fVjw;EL2Ad>daz01^E50j%|Vmo!_zM&+Ef#RSOE#JJ=9ahI=`?BNsFzG;d% zp7h8sooJp;6F-j;@kbMUCGfOlhtp}#N!)YntMo|neL{yVVrM$6$ykr2M;I9R5s#jZ zctfh3vmR`t?ir6W>zCm#%5o~Y^r>NHkDrN=x~bid*%rcLh4Y1J<&Ex3_Me!~_{~|E zzCbkf@u{e=FHJHrStX0rP2x(-DV9Xn>Qwo{gW%&~RrtTh}7`n2ttIRrX^( z@K`e1jkmJHiynAL&T;mN2Ly*&bP;(`#=A5};z&}uz=(IwmbEVO;%5eQdg86-Gw8Pk z!Bp-?aS=ZR`LXb(-FG=HZ_>l+^ZKr%ayX>SaDZK?M$>EddAwC|o*^elt!Od+%Sx*^in7&I>KwJ%3v-1k-9&b$F)!GL87vQBgJ)?A*m< zKK6@xrKP#D^u?V7gyFTG9M#!dYp4Vx@01gI&lZVuyMArG?$<_LhcZZQD z5C(`%eLW287WVM8qU~N-XYX~jX;b47%+wAKE9gx#P^dh0tvh_2_tgzsq`Ot9^1yvY*fE#ZM!9;}$p0h3fFFk}r^630za z>dyznHtt7CaD>?abesq{5rGlx?2MEkqkkX;xE}VWHS$(^rRQ5VRl;e)LE^>b#D%4o zCNRb1sOzZ;R5kv-lr>Ua z$>hUs)Dz8u185aVpvO|)wWi*#@|JA8Z!|^$=N$oDT~q`6w?} zZ{;9YYMxR*{yJRtnt0hWrM3^~_5Pfq=f3rs22jSRGM zMq6`g;W?WSU2IJ2nxHjMeUjQdXM9#f<~Fp{zP1w)tQ~m0DXXwB79Av>I&)A31R@9FWsRINQ|hi(RXU* z>C%s3|HP%=@j>Ar-hg3u@vhtEovP?16(9}R*{ld4I@7Zng9NO_$>%v??5S@v%kc{y zXYiWf+`XJ?y@kY(nj$Hi#~jar=u!BQTh&omcxCn=YudwdQi1=0^?fVs1s9o6p<(js#zsO?`lR>SYTR zW4(w?i=*_HUjHl^>zG1<$@%;#hmHu(UIB3{>e@x51hg7ZuI`xlRjvj+NMy(u9s)W) zNI=KnKm>|)?U##;GTl2IH=f09<_&2AcB@Cp`q?rMvDeWH+y>gSH@XW}saHYmGOsyq zUr@ru2R@T}%5HSQbHn)_fRCqiQa<67-d$Y7StQ{+m{#vsoOf`V)5sxli{>d=PuH`KzG-R)&CWjFMm%G+Udh9i@EA8eTb&uX1}bHx=g5ZnL6LhX}mly zQmLeVnGp+k1q}Y+px>$?0v=B1nvrzrG6;3^g6|UrjJJb9GTi|*_mf#haMJS!U>xeJ zkO_JTY(X>x7%y^a2G-;ulpsnxKKrQNU8U+*%6{W<^5Sv;(zgj4h@an8fz}l?`qCfd zNDd<1s*{5Cjp>&n83IheFL*TK<7`%EMxC8MD7y3?yd{7IH$*?c%v z)(v8N34E7v0{9=Fu1BB$Zw$Tuu@U=A#{Qq$F68bXJ^KIP5cYrZh01cu)@PQuEM_}1 zej4m?Z@R&=CM#{dgNXVG(^@7fg+&9VPf+qzSR>bX^Gw7b*OOfM76B}=i*HA}rj_MF zUdlKjaO60!wob>WZhdH)>Ez*6VESaYR=ust501hgqtE}Z7|K1DtNRxker&HKZZ(W# zv{JG{hH|-^^ENg_#*iuZ(g#P^f-C+0P3GZ8h#N@=Y$tx*W!_;UU8ddlyvHxc1pb(Y zof!8B)2|L8^^Z^&nVo76z^?l@)!TlWb#?m!jqhZTzL7Sv3%OotJu(;pRraiss)IPT z$wX}uw&`Yv4W{vs>&+G3k`#>#ttxL0z+gq8*`XwqsxH9)E1*WlRG?*4^Rtv#Dv={K z*dI?c{*EX$r{yU?!p4C=(pGU&qg$!MDqKJS8$O7V(l}z!?y76@o%WQr`=PFEhe~m1 zprj?Zf!(gZQ2y2}g?jpPL-wo*^{GbODNe1uQywaI8}dVR_a+oUKn$?^(75W`*f|X> z@uNZOlN0g6%Or4+tY2sMId}~(sff<*_i@OZ(L}$RHrK=x+}kP?W6Z}m>C}eh<~5rL zut|byFQ>Y6`n02x{$WXQA>UU`d8OuMm>BMKVRQHF43EwEI-^?`)eD{9 zN|kKeF`ed$R6@><_qY6FKvL-b z$0MGCyMz2UP_HRR{{TcaS7h8S^Hnq=&2;z?0DRO9fsYhBu$8X5@prT>5FJZDP4DN7 zq8j9iw(Fk~2xAr>v)Zn9dVj{Tm}d64a}8wJ;5B$De%X202!N z8g^rzR81xz2erKxn0hGw5@sb(6Ci+@X5klPb63s+<1ULVU@aU%{GZ}%fmqgh$a$4G zy8EjwIV%D0FBL95x*>Vsz!<)Nbw_d993Rh4Jvx=!DavAjJ7m7q)DsY%Pi!v@R9H(c z0tK`zF>Aja%NY_nh~H-_-R#Yy(y-U6(p*uo*nda`^F7;?7y#NEXq$6tPodbiSgD*L6rNB>(F9*b5X4Ih;Im|bdl zGrm?QdqunG{0;O5jCZ(ROQ`n#SDZOnB}DvGMvQ zUcm*}coC^F%9Q|PQmg_gS+<@TJ+)1Cp=BMepV(c75Fq=*nbQa+XEhh+ldhK~lIZ|J z|I{PfF{R`CPlHV)@CynC;c~$3Xv|~qsMWy9*xCkg83fwQd_d_I|I@n^w8?6@@>c?} zF6$N-*2mvyCGU77Wv0 zlAvI37{o#(!czxrLxph2UQ6e~jPSd_vxEmG&2W;v7>Q|nE!0OgkS{uPBLEXX3QT7dK#p#h z!jr2(GGs>lE{s8p1QM|?&^M8!O!TC1519tAxT#^pNT=Iv(m~a7w=)(~UPWb&@(g3M zOSQOvs(w-4d@qo<#zyN!WvI{EtKv`4~Pb|6Dr~Q`Hw?*4{cvq<(D3^u@57=IHl>I7(=F$+^?@ zP`6#LFvT?RMnD--eRdzNe6e-e>jBK7`=}TKEr4{K5tn?vpZ$U`I7AHt6Y?fL;(~=Z z!&Y!{BP<2do<4I?{3MXFVZ1xA|AcC{aF94jzlST`##l$rhs(%*K;dcy)E19V;yc!7 z#LrZ7F$*=mh*=vXq8AAzFvT21vXO7ns2%jFK)ff&=Ahz{)!msX#Josjd#){AjA5 zgJk-l2rKdK>nq8mWu;v1O>Cje1x!cHUlm(k*9guH|9DtbI!`#JTShX7*?YLooIq#4oC)PxjbQ!)&26TJ(rZ z$}D}=#qbH1ZsG!Rglf*th9-l14maqQ7;s{n(eBg2r1Mbq{cB#c2W|IkpU}c2c-qH0 z3q(~t>pf&SHmBE5Og)8fkI3>W_@duVXSLBLxl}?!3p0Om^$>q&JbqA4{IbA0Wch7@ zrT$@}mOLt{!OJ|r^_s+Lngto>kAKx4$?U7ip6|gL_2*MayexbHG~suq(8I&9rfi*#DVLXy#o^(~ zk$8E+seSX0h1Yf9*ADI4c;Mran{r2O43+rF`|ieKMQA z5q==VooF4VU2y~5?WOud-Nm98T6Idcn7g%v>-zKUW67=Zp&<9jsyq}M5usxXiKIJ(_#CC^;@tT~KNkw6_@{JKo2l?BzN13aTFIb5-_SUfnf#ml07_Vs zyfuQqU5Fq3n`Ml!rx^aL&a96iRW5M~oT*|})wvOy|I#0xt5gv@W$)jox%V_HFoC_q z#9#^fkD}o8qX1-+lwKdnkS_Zi%5vq&R=ttmKFr$Nw!&ZpiNXiI_Tmn?y~;B;Un~FY z7aOISN>$Cltt_n(ho5i%G!wUr;)m3^gOa-a;Q_Dyx+-N6Mwe1~ZRHCxHA?*g{eYs2 zE&=w+PmO8KMBESZgL4YV5|vYVr9!-Rx4Me13PjHh2ovjY69Uz(?FF*~3pxi)Fd)3f z1<@P1xgWfK^*YjIuC9yMLgR&+{WaGolNp3maOJ9+jG;xwcIL41O#X#GZ*9}?QRLbk zM|MbM89#9F2&DLGLxOCtpyImUAU_b+VnYiIDcvfjT$chge^8%XpE`m9yaHa+AJe7) zn@9JvEiZD7Mx1&lP=N0C2T}3Gp%;3}r_;Dzn6K8-BfgiFz=)&!I z-kTs%e^U1pG7av+8Sv%-1PiElmJP|#8~37nS)Xu*#`*vSWJX{>LvBgwP1^Mj5NFRy!lO!!Cy6Ss3P8(wPq;9WR*|t`G z*&n4-FJucNt!|kT#M;_h7rf#BeQuf8%PYq^5Zf-MdAW(%Y`O&Ucac13~FD} z>bMFPbz#Q9ymlWzAgY+4u68B`Ox2Fx(Xg2lG<&P%*po~`{q%B3jXAsbZEM%l^3C6{n%^aCE%%XcaBjV5=F6Yh8 zmmq21#@8k8(mN|t(k$rpGizp|`&ZWDkTH+asNpaIVtW(KRdo)r2^M6#U4g{=iP$(3 zPinKQHrzROj>5Xf4_!CS_Po?KG-@i9d>D6ErXZv`cikRD@wtRu7YygQv|l=m`H>tj z#0m*_+@*Ehy<06yUl~bXS<%XT#z4A%H}aq>y;^>7-PQMeh?<$#vj&K2cpi8M#FAgU zgFOo4uR#0GcIj83T~%OYu}6|U67b7B$(*2}&Nq5A!*}!tseRurbp6%xskXXAWEjLr z7mDn`s3|}9Z=WX7*3?67ro*cSc63kp2k;{u1O{CWnY<8ikXiFj{UjZe0^iobPl3Nq zTyFQxcUd16W1_c>bt~)I*k-OjzVE6x`)il}Afxfklpg1knp5sz9W+7^DL22PNqBYJ z_Bltr#63QLz7H#_oo^9+1TcPYW+6>^qSo{0r_Vzy5RIJ#Fzp25m3xX|-Uk<1aya*O za3;p@?(J2Ty9DpuL7MV@7+0$%(|xMy?GeO~=*j-POr;zIYRD9cZ`M!a;XI9SxC_j7 zKM=yCO&Oc8QX#@I^6gmY@Mbvy5xvy98x%`+N?5N(It_oQ3uni&G+kV0-_J6J5}j2b zy==$g@yJnHyJAeG^B_~qk#>G?_>H~GggE?0Y~wQ7oz9B7gU@X4`P0vT)uAJ>+9=_z zu$|F1zXF7Z^3*sij6kR}I-Q5kRt$%hJo1ltS=+t(McOWT)VYmk6ys9oj)zw_N3R1@ zZOhVV(ul|~gA#Vsy?S9a@vJ__}y?GC5`BgJuYzy!ZGPoyz ze)gGEV6Tdw=F;KbNUZ=&(8gC&iP=2Ia6QPX!5*t(x&~=$X(0UCEjkeEd^jG} ztaA-5l4&vGWPw%iww#9qH%Y{7aViw?3a<1_x3(#7l5!QI;uC%Jzjs|%aZMNFTz$_B zmLqO2hcFP+gb19c(@1cy_E>SJ<3jg?g4=#xcEmHyIwDoa_oby zfCj5M+&MC1GSFO{77?yP^nWLj7Y5wr4`Qz|3;|T!3h|F#RKNXkUB)S%=hX#or}_bA zJiE(mhXAgS#{RF)SBC)@=fv`IyOz}bz`BN?s?kTozNE78nZ)Pm?`H9XO|DkgpCEC3 z?1~|%a2n;tuTVn9L<^wI)kDoxj*>k|R!= zlvM+&;;_eO%ul6~TXl;1rJz9(B5-MA-kS*E-sb!FT}!Knub5^HQw%#^bFOA^Lmv2i zS^dGN8QyLTu@O6_Ags8c+slRWnv3zHq8I7Jv{EFY&b}lj;jF->P**^}vB`+T zfC4i_4Pp!%2MBJ)+7w`d&ZF#oV9QONcepQG*vjZeceof^AJ0myw(jEUD+j9V!x!H# znY|nwTKd2~P9DlF`Y820uO5tRnY{lpt~Elw+5<4~K}GL4@|c<|FNVloPDzfe8=z3>CAFtc&{j{Cs0UoT!rDI8yKiF&5 z#}UT`s!S3FQ zX|jaiKQ6o4X!3N9y67M<><$4A3{vTVFEz~2c(5#@Xd5sL#_}?xG-G`T&~Tabf1Gc7 zQJODAqLcW?)pj1`!AIvcIfF61aJN65mLvHWhm09%7HrCf9<}z}ohfOW(-EgKa*wQA zA`H&Gg0-&9YPofYTVn1J-NCydt+XD7#HF!5QYfeh_2eWurgf9Sef{UOq)Gw)YJ;UwKS7YzFxS7WZ*YH zjWGS)wV~$uqpde;U#)i~PM{pZ8F)KAkRXFMzDP-u=nTYG!zUe2XVUbF5k^bm+Y+d| zNqhE})&HvOqZ-)zQ9k-MAWm!0Onv;30!<7uoZ}T2=Rc!4#dZBh-X%Kc7}XY5Wu7YQ zk$)+U%W5iqeVG1#miily|DQ|!HU0U6e^2bd?o_Sf?-iq#iBeC*rJxzaWWXG~;R~i! zd#LYQSU%ME9sa8C)06Q4Q&u;9=wLLHg@57a9XqC<8c2GIt+vmupMJDH>k}RnN;iij zFOS}4Q-F+i+%(p%|d8cfc!mp=Hs|)RAg~7CX+b+5-VjL+Ogec4!j2ZrmSZF2i z4njO<3ptAV&doo{@*l3;3Mkj5Hq-N(nTi#$+r~wB$SV)MM>j*b5_7TLVUCr)8rO!Y z)N(vQsl7N`ysY<`@jcbrIc95oqzv4M!oxPE;*)!f{cfRDw)FX*D` zExJruna%!O#(qGb<~&;5E00Bv2ze46(a`k~rv|H6{RaylIX!z@eL4UuA*SLmVIni- zQvFCwk59Z5PW{o#MKttuO8gOd{IAtidRTXkh9wqnm@JGoiny;P7I*^Ft?mUeWNs;# zuh*=cCk~o;QQL=nHD2yG_-T-`@0)Jv$(^=8tfI7&A(VjH zrnTcG2c4iSDlP57Xp;p&4KGY7UnOZ}+JH#&PS*%rj&yor#M<*}@wy+;K0bY1t5eKf zi6T91Ocn52i-@zcU*}uO0tSA;)6U)>2lH-DT$Y;%U`w{k9b(vHRF-Ioc&$UcsCtjxr|3g{(iERI#K9UySZTs7LHn66?M?$RX z3hjFPvV2yCxA*g>hh%6Qk_;VC6aurzfdS_%Q1vR6yXPZY{`3kL#gzUfCrq+_e+0Ut zoFF%Ww^{D!4{kS#Ag&$$T@n}l@L@<C zS}<)c6=)Kt&jKj(W8)=7=y$3U#U}?&FS9WrxG7R`dIx%UZ5yTFwRI@OO=SG5)b1zr zFyw=@9~dGq7|MP|BdU4(AJ5b0cj^$Mn@*#37;->bnF4e4Exg_t8`aR4}~*NocS6ANw4UMZTv&Lk8X`6`>zi&LFB-uFQ;F$TA&N^VMv}td8190(jfc zx36mgDJugL=2g>?iQ!t2%F2*C;~fh!dnvF3uRD$cWFKW>Yc&Ep1qNGEel`>qya zKNLc>8h57!&$)8I&yU`?qj5RAWETk|iIRbGTnvB6cpGt|QEE9?WWiI&LH6%2!7uT> zmVDKONd#^8i}L{(WS8H`24AsO4_D-$Z7XB(CW6Gr+IiTZK_j+7ZwCOSu(R92aFhs{x;{Z5?;v(_W_@qQwzM-*w1oK2pAB)~`zpHP@q$iMCxofAoC(+T9TJ^W_qT%W^bG0;Wr_u6E1DK zHh^oyBpBaF87NLNRBIjMIh(YMF@bU`W}==5nk_lzr`U)zk!HWqa-PMO+i=&99FEt! zsNvB;f_(?tw3RGzLeFRp2q+=1dG^^1cJIqVr(3I1pB_LMLzRm_2;+!7ginx~d+#f5 zem+yL>5TtF*IS0g(FARyc!Grh3j_%e+}%CFWpUTwuvl%NwwZ*X_UK!7E)ues)zf2bk#s9!tkkaR38lCdB+jyn}Ak2Wp z$0jSHMJTSTvcQB~u1=`^h8MSknnGm#`xzJoDBvGCeBA+-W@Jko8fHH1n)qk*{!Ar> zDv7R_oBR5FFwHAKgt55*7VPi#+Z{gDZT@$n5~l^(v;cPA_w0bmop z{6AsE&P^nE9nspt+DW;YT9*u_)kc(vpQf5T7Ze*2cX#DV5~l{1s2|%Kf~kuW`)^|X zhQ7=tv~0rwF`*&vC8Lp@hZVUVz~)7nJ&FAP3|M8Nap;n#)z0kJsXrz4%WtfzNGRr~ zUx)L((3l>?S9f;0TvIv=wMFqohwicszWk$vQ z1up;j-4{8;%le32_!J}bFL3$iO4a>w;h5c)dNlob?Z0z^UwHBQ{}XmO5mEOq4@I<( zPF$B?(sZ7(?XP!i*udp3e)iSa`~4l4a9zi})#ZZrJzQ`m2iwn*-b39>Smq-#A{PMi zw@;I|f>vKQ^ILcZJNf=1py2%PzkXBRmM5WVfpJ;?w@e=1d++}@zOixp{G=ow&9uox zxJQ#bEioY1%#%wOt}p!Wjc1N3EPzb@OX1K{68^rlNUY1~eVZHhF2!Pt|AS^C)X;gd zxH&vVG*nY4g(S0*!?n&N8VF(5L$CLV{d{&$aygNuXr z|2@t1-)SKsR&~!W=B(OECLnWD7gjY_6PLfovUbLn=B!%QW-eCby!>3OGUnEnRxacm ze7vlmtX-Uy&7H&@?7leIo7=mP^RX(J+grj4vh%`fRYhZ!vbJ?GcVd;YHFhzVFgJBD zGiMcdfJITNeK9sQXO%E_vohyFK>(8QU**oB%SL1qmEUzw_wJG&^mn34ZiGqnNV z6o^&Q#SE^bq=4dY*Z$oye=~<+UY#G3yay1Tz(Cr6w}lZgmE2k}Q3XxOAnSjAa5y-< zaDpo=Oik$^>t1nLRP^(Ut9V&zANbtyEsnbNg>{gTj&@iEg?cg4zmt@*&ml1L4(ZP1 zAQ(fmNq=}x;e;qnDF3GOuFnW&bUZ0${Ipa}6~oRgRAHn;K)cPVHYlV8k~Dnj$ueJO z?e$a2;KP5A&=!HO8Z@d>fh0{2!wnT8jPz~f`oaf?+%hIPLDb(tk~Q1dlRrwPewO*- zKRu+!f-8MDTK@cn?0FqY0f_wSJ_w~^yx>ntkXPhP>9rq! zGx&L|<&kiGTPaqu+j7u2w@7I}8s_bh8$(g|8nsZ!?XcR&@^tRb=JdWl(wV7+wR3@X zrc|l7I>k;*-l~X$Kd|TX1ya|+ub=jgeM8Ztge>7_smr9vTS@P5@wWBwR8cW7a4r@w zXlQ$tqMnEWMN?ME8(&B2*~BZ+dHDMm2Ku$&-Kcny4V8>CXJo!lEs->r&E)_}E=O4WSfv444f#^Hr$Nr|N4*y;ZG z(6ZT$K&njgTfZ93JHjnifCS3DrUJuP_-BsRq)BtmGcx>4O8kquS8U{LG8D0fU+Au` z2aNWlBN2U*5pw!jsg8)jj@o$%VQjB${92iqk`%_!8D&2OY;OFaL>$-p5asW$oIhO9 z$JpgRQU|CUb)r@!^{m;(sk{wsE7!DTWa9N(ql9ic4SN)OEce=h?82-^Ew06h*&oC- zB&JeF9Kz%bLnKX`EFaZD%(>yN$SV`@XpyOn@Xy*qJ*Ew3hG9SUF2o$yD#+V>kO4Amcf(QmM|31xLuM$*KPMTkU9~A4coq5R^Az-)sQ8PFG`DUtJ~AaCJ{>-iPiB-o zUo%s$^cXtkC9Zr@ynIalx6Rwew;xzJzAG^Ip50(!Ydd)i5Gc2%1aRwAON>ySCL0Fe zy(nvqN7!L@`SpZMwo6SkKoHdBuD)@oL@aZ`e?Uk~89xgBW^(& zZnBJ;uF1Shom(y$Np3-unpo@}o2U;UPC?zBA8+mIJjqVPgUI=io`nFpJWSbUsm#G*g``86hvtDzTZ@@mY6T+YV<{9gxY)Tw8E zk5`JXU!Nt8?CA3(zU%s!itw`-4h+UFA-aml=-K@O;HdK->Nzz<3-0(*)j@|6T}g*R zL);!sc2GmXNz*?W8(Rw4XT(4|!yE0G5+yN2%CRE1h*Bu$MM>{V3G>(p5~gxXC@R4J zVcIpc6UQEEiTsBW%2B7kY<-!9haTdpdFE07{Hg{Q3ma<^L3G8)uCk=%13aj1K?aC? zkDNX(69$gUOX`bnZy7kL*B6tzF=bHH-^QW|9mTxFM&2r>9MtbKX1|KrlmeMAjB^AB zC;_ko>2VGWEHfzB4MXuvto&nhdD3E?#v?DaP;Os^o%O48*t40hvMFYpE_j5LOHh1J znQ6f$qTeM69wDfZ4bsq-j8K-Bn`L1_IZXhWij4MCHxM>LP^7E@7RQeLxp+na#a@i{ z>W_^d%wmZ-#OUZ)5IS^z1t;HDHB$GK;4QaMLjcWbeT`j&BBzha(KX4;#9uXunaUE@ zc8XBe)v7QizY<2#oF=5~#CSmKifI4MXPj?wy@e?t zi37wMPpjLOfOL1CaTR5J?fdk&vvi_1_kI|9LzC9K;WqvGY9{n3VT4s?TlB=8YI?#_ z|50rnv$!TY@lWAV#7{X)W%mBahXr?C>N5S{Biygb;(ayS=Zu zyVutQ=Y3qDcqZG;`&^|gerI&n@rog^K9AjC2;z^jFEvl`Eynr;{H&iS`%K=VC9@}L zJ-O|Y$g$h4&!w-;(8PG1=plKJU7-thhL0O_@;MF=WeH}a%tIE4`ZsuA-} zW(~0}*$g!F)}8g5HnfzaaRbY&@9wG(iV&=@zicxYv#sVKDJL)5Wep1Rhvjze*hm$f zD8XrBq%@xov0k0k);g@=%G@ERW__!HH`o*T3|q&3pAoqjEe+w#43CsyR0Bi2r$Fw#S4;QeIUSZ@Ka60y~r5NMz`{ zfskVvH?y8&7EyG5Ekt0A#awTH8Q~G(&O-OwX2@tuYhp;cFR6x{sAqF)cf>WnmA|mxjfm5iKenEpdQ}DK|CssEFh#e*P7Yk*tC+<*(t0`j_Y| z#$Pt2BdM5E|;tC08G~2$xJ4IdC;7dKblEx zS?(Mb<41G)TOmy;BT0K!LwhBb!N9K(MaC*l-{wc)Xs#b_KEyB3_ER6_RQT@rkhb&v zT43)Rj3O2Ag1&``{a_5C++VRTK4E<>QWpa~);f@FlIl&XP~WxYj^*7O>koGnux4*t zKl`bwn+mc%8yFc95`>Ae*zNDpa(SfGlp2gO(a-5=T{au&FKbY5e`a@?8v4TWG)5+& zcRuu}Y{X7G-2bj>tgHt+GD;#$rYOQ7USh#>PciHilJ(jpA#rjE|!=Y7s3|&?JQ3eVfcOIIVAdz3?rPMVkD!IKsO%&qb zHqXKqpD$i-@i9DYMl?l4?ob9=+eD+6T|Sw_Q+Vn8sj4Vt&Ej`VrwbmhO~;nC zz;gv^7PKAoV3-P%QaMc_M-T^-LL z{xE6CT%Nc$f7OeoPREmJPO93l%VDL4*?)-AB+T>Buqj`wo{LkC6UP_b*HoTTVh+Ij zxM>!hn?(8cAx(GF4p5{n--}v_(VX_pPs9&CX(v+jbgs$vBhhIAK6fpe*K+^2>1Ig3 zPZvz(QnRS2Ns%Q8#jff5-G1_+hhCYkOQZFw2dEa49E}$rxt!hc75g_9Vmq>1gTDF5 zX4$GIeO*%(^x{>Z&ZwcH$q5G#nH$k3-(5eu{YWy5XTO-lr_YjUq7th}V1QDkCOzv$ z%TyA!`Q4nG%BG&9eK9kR(u`tY^Q0?(UGXMR(w7_5+CA6~5Mp5zRE(P$uSi=FDR8YL zqFHs8j3RniKErXNP3>cN!)Jd~i{1AMCoQ{{u}i>jg)PE=0G#P(@-B|3?i1xX!0D7&1l&)xT zbL^aGz1nA$Kr}hYk~d>JX~RLs*2x{n%$&ARaf+T5zjh0Yx6WIjh#q z@tx>OsFIrMtW&R(182{J96;t1WOElz31F4CdE|&&#}*+ z-Q1eIue}Aq8=+s>HWfAwNr{M8phaD zKa!DEeNQ1O(=2CRHZ2ro>_Unr1ywfx*#&X=?W)!3%ZF3Yf$;svihDAUol|ZfK*M@C z_GMBk=l1La5%)BW)#h8lHkYzxPmdLO&EpFe(;XKIx;$Vb=JoA9v8Iqm&u!Gl&0U9u zMsvS)WmMXQ8W;M|iemi3QYjYfI9?k4gg?}-^>rEbHSSyK>IrX?Yao_fBA_~z>rQI4 zWNjv#ph-qeKI|@&4yGit)J8uLwvixnUC}H4W><-@cZ?E*l5`{T8o?h6y8ODFs&d_7 zy6}4EB+J`C_|)~BoR4Jl!=6V~%&bZG%`0yJIA+xuA{aVpiuZ~HXC3@I?pb{E!B`=K zYY?!B^i{Wj<;}tvc*#WYO%$ zaWzUcmU4O&=*=WPp=30VTH^hw3=GGZ1*9@Z7=WvW-=#xKwSkgnkR#}bN3m)=Jz~(5 z{MoC$Snh13e7UY%U1p~8NdiqtPxKZl5L$bBXzSFZ5@n+#FkD7Y@yIjvEZBf2zl7ZP z1y0sKK;A&C_+;;oMkm3u%fzt%MY|W8PRMwB0OOyz-{=(14{-Ff)6$hH-ZoP(mIH>3 z*qs)%PZpKzIYz&4Q#cU*ab~Vi1~jS!F64CLv1R1xR>Xn(!tLd%~Q{Bf}nt^YKJtz znm(v0a_ll+Av&TBklLz>VwCY-5-3wzg9LCbLE?2wK1d_DeE?$)%G*aZtjD7mq+1@_ zz~H54KgYAR9JNGOV6H~k{JlT4n1cRF=zb*VlZX<-F7h3{LWM~l-j&*icvUVMi-ww> zO)5u@(FJvH@8&SoWC{A+%roRunYa6)k=Jr(u@-H(`Ai7v^6b8)7K- zJ^WtS9KeJ_>uawoULywkLPa>Dbyd*fzfbQRSZ8l=Siz;KMv@bsao`TO>fRRrOW zugn>ZgnuWdHX=)((@*}+axNM1vsF5d_Zu^?-;XxRz?sM5mS$(F>--`R|_v5-PmYrG_t0pj^ zkDJF0w_I`mB=WYsf*Ez6A%Gnd)zZ4kisIr-n06}rp_d@@VP0uRtH=I>4I*=j13kCo zbiL!Fh6n<@62+#KZ+TX;_^c{L17O^{D6Ws&dQrU#-X{@DzS#Ux$OM=YcP5^vYhTS? zr!R_UQa;#kdoAgN2}z93cvI)wnJLG_RYg^D(wi&Ny?KNylFIOe33x+0Catc2GiIoI ze|zhN-qhvP6k8KV8M_|pF3wn(gpks0fUWr+CF1_RtO&$pGdOJCi@i2hIdR-TvC3BM zZcak>&V@B4_$m5-B?;rkV%V&A)1N4M@;kia)4iWKrd#Zwk|h4mBS8M5Kj4hION8?L zz{O405}tNJy5P@Lg79$st?h)der_rex&es=U#2N^S0-wS1b@;&f#SJpqd7f$`<58T zwob`EfVd1NgQ)Ow>MSUmH%odJ<_-`e4Kyey@vzOY@ZpGHusaQU%dUiTcl?p@XggA} zUKMhCUyhWPwItag=IGLwW@Q8)*)yjg&rwsN-FqUMk9vdtO1gYOfhMsJOT~CyR34h7 zIy`;rbrH0N0*V-?{_%RwiKug=e;YYwRSF|>5^Y;H$Y}(D!Bx~q5YJ zhJ3#~Fa1ZS7=_}UL-fs+9{PN%!N?w*;i9h#yex^T-w(q3mvhNDE*hVSjSgb#A|@D> z4!YrT^hhZ;KMMpo7_5F*~pP|aBB9vGzA{ zaNLGytH}fZ8UGl4W%O|1wWzKjpPO&fZQ!ctjqrcmNNlAOsmH!9zK|1e0)uv(8?U?mTJSEP!^s>tR^>e)dd8Wg7U$#Gv*^MQeBP6<0!d@AkB zxVaINYMj6#xy4aty+J3j5zBl=1y!-|)@c<^xqLm@uSg!hB)UeF28S$V4cTs6uOE8F zI5zD=QXZz^#wvdhLymi+MMWgH7>)1Zg9^fa7NC^Lz(lVsN&9?MAr9@Xay#ygd!NB+ zNK-5EM#^dv5QLpU=iP0^Ou3`jCb) z{ThxxFNs5<3ZA^lJR@ePY^2^;Lnz6ORe(&to8`@)+8W&Rb~o@+UysJGnrUzIjU&q) zZ;}H#dia>va1TXDnY@gR{nWxEV_$~8BTEjTJv%w?BkOovV*^7G~%=y85Kw1mAos@jTYA0jtPh`$LoE6+FiNryIH+~rxok@Hcx%rIkxzu8$M zZp??tLihTBoiw;aFUBVfGP62mTkZA74{Bb-;J|s$tQ$hlB(vZYa9JH z`@Ai14#=@J5!v;BenQlJg@qd2XWoH}!yG^uA)od~aYJJat}b+*9tgmOLQSWs!NQM{ z3S`MxSPwL8uKN?d-Z4injt`{zI!|BM1ZTW5o7JdqxHHj0WKM<>rDLA$x29$wl33~^ z>FYdTQJG^#?0370%-H7|>r5-2LE3(ajy~gTqAaPFMI}%M{t^~BC?NNE@7R=wQuoM_ zAXstjb`;XkFM_q7Zt3QbPIyTBxdH%Evq@ec5U;y?couW$k?98xZ?L=>J7O}4zm*=uE$70$$=`Onyxy(zD40O~ zO)Ymh5Z!{QrauS8WV%vyRiKM2+vMGvc%;k^)n2+=UL8`xu31`bJ9{`O4*VpUwK*ra zrOx5E2Qkg0bxm{*>==MqV4xiePC`IKoq(+`Cg6v!x%&0O)}$Q$$GT61qg532O>L`c@5Wu)flOT(0(8Ce6eXkX}U0gb&5or*qSZ{bn^y3@r$YbI{#mjE4_cTSJSNqi2s)AH(`J z6LZ_#`hJB38>)cye2NVwI4YOuU zRE=I?ljh~gI9+403$*ORdNTdr3BngD3z&Izn1aIwust%lQ3j$I_-m|)1zoyjzV{X+ z594t}ODPtOT$8q-+P!nNxq%~NkuL;fHmE7YqlK(2XVSg}z*uK38j^5QV#Ae+(C||A z8a+uO@ydSDC(Ds`?PaDZ-ONhUZM~N0$CUVFMtDBh`=@1K-{eLfnMu?EHNA=I23N~m zD85AsqPn0`hKl639yJpwY#C^_Rr+;;FI~Rt{B4-3> zcEUTu*>vgE=q~8%a(@8+=yaha?{~!rk?v~1OQwul)V93QGeQI&_i*gwi82-=4LIQu z*oh8f#MRSfNomSMGLajqY0!J|=ol85{j~8+OwW^_Rl?p;$nV&gkJ7}IFZ{M+P3ium zT-*UfqPJG0ci^;JR1k#@n*k&R{`$_tVTka@1(S(|=()=v~zWV3ki;%$ib*KtoS zN0GD(BQxwjB1i64C$M#r9U&Oyr=Em!s?5!@p-<9CWq7Xuq~E5X7O+_m;(#cBH^oeD ze36|M04_7lyf7Z!xM>`I*t}lYQ-%0bkmn9*G8FZETvD7eHV}|D=?)BNTtlP3_Vh&J zGT$JRfg{T;xx)Ubp&82H3|M0j5$%^U1?b0sZvB{sgQG6M`V-_`E7?m6@&~NO$p#F$ zsY)UZfGP~%+x6tq z8_AV?LK?wR0RAxl#~*&;sIPC~|kC+K2-goB!Mt zD8^{)hwmARQ%NBux8p~mGTGUW6vtk=hfIIxqGA59ebJk-H)Tr?#=pX)6TL^@GE;>w zQT^AK3n%!SH7rr?)+zlT`W`sA+CaYmzVI~KM6~}cKpTY{*`kU{_V0-ZNK@?o z=lXT?htSu6?EWkM@EaZU@2%hA#en}7;X^2c$dw3y7s^FdQziZ|tR_qnl%{;}Ybtr* zHq{WL%d>?{1(4~+t~a{w6rDgLwv4oX&z1jq`WvCAX!LPXu6e@&$UHl3)5h!V{hDDF z=kS-8hG^Cq7+1gY;8|kjz|1_kbA*@rAf{MQ15L;Zh-bw_xL?2P3YrVHS?S zdTbFZ_S+z|fnCz_IuFQCW>_?KyfEja(gy4(pLqe;p7yf(2381droJLG%pxuDcR+$u z-oM1YWNIEZ2!1?oS0(N0TM8iHjZr*csxsUltYpc`D48+H#?{7sVmw>D`lR(m`PkY$ z=rScRb`4zP@M}%9J+tFVk3H}tEEoVPZ~J_7^5Ae8?(31`Xd<5_^-Fz2gsVV9VjY~5 zrlqG=xGnBh5uzsL|QHPKD$!(U$DKy49z^S2*Q6U@Q5T^MOg{^7iomoZIE70-vOb=;1!Y-ix-iY|=r zQ;HQI5_w9Ah3FgcfklYfT%&arFH;KD^gB&?c+z?m=^kTX1pK*8-ugKwUGW!do5@`q zrQOn0{vYy!^UVVD+-40g{Q<&jA8u-WoPi-J5LAN8)eLh@V5^O^;o#fx}Y!|(P#@JQo7KL204&`I9$k7!}MZTs!p{ zVHBWIbLJ=&=ukb_3)d;ee0{(zTh=5=_EUUR4QB zPus(8{0(3bKX)*_r5CwTRhfp{EDW0Dy-#_I@}xrh**E3n{D7^%(%3c2UH*oL|ED}y z@BM!5ooutwul0d7SV9=V7XR6<-wC!00jBO3Q!?X)&|FLTNEA@kZ{KF&7Z-ECur3W- zVxhZTZ5dNAll``6N?{?I8UffsDX0M}LfO%_bdF`E9Q*-sfW~ouoq;{ow zBeH$NoOfjsDZfAmPx(1s`5H6cJ$S1_MPG3`-j_80{$^#jS*)jIge`7<>Wmf4ZtW&G zBG7Vq&`tfg-eS_UwNfy-0M61@=j=7>jRAE*+-4+4uTmpmXe`1&UEu7a*GK^C8=nh2 z6{)BcS9h96Q)FDSx$nt+;ioSRzUQK~Wiew_Jn=KV>dmPrMNyG47V14 z?gu06LN`>$>6ZCVGN+O}p7?0%>{b&glg{7?LRanN1bRuC&JhpvwaemZ-c zqV@?X>r#V8ei%UWM^`$P){UM-GOWtznTlG}3rd!(P2ELrZy)>FFk^4mbmf2=1e1I< z4=z4&fbJ3OO5Z$hu~?wo-Fu=?352cH>q&3yRNDjWsyF zoV>^?+|KJ0;ZQY7Va``QZSI{}ctvZK?)REgJKF*9@%JY1xn~&~Z@LvL%Xl=QTrI0w z8%H6C9hqp^%mC`t{iK!?QA$GK1IFWg`0NYuge`~3if9ZF!dgly-#?x>aj%Md(G-3{ zp`{j0^uXeXFKcrbZPyRdns+eq{TTaV?FBZM0`fP%Rea?545%(4;=R_nD%M@?4G;zr z-BmxvCfE%eK!_63!3QqEmMPXjJW;Iq{#;F|4zE{vgG8-PDWzwVv@qMq@QF+3Q`#+{ z0MAhrI5AIS6(IWxt^V{r#}Gq$N$QefSHX+{;RjllDWahx&jRl*b|UXaou~nKS5MIh z>!o~f*NcagEz^|6p&l z@NMfl*>R~l(rJzA8aMOy>y+C8_v!;~f1X<4gm=z?AsJ~2ht6;F+vK-T9qlc0A4?BS z$(!OvJU#)B$w+vw&7MP#gjda}*C{ZefX=R3o0nBA*V9R?xyh7G(82i|o&8w?{P*Uf zu9-f~n~?_CC^J5G>;~sGcRAa>Z#{DrMP{_T7DnW_H8?C`y>)cjb!#W4#!`H)8BBrp zba=mv^ITV6RxvOO?Y3y<)e$!=-G4mo<`by@FxycSPIy^EWUKoyY5rI+Rdp~PI{vsh zOuoXf2V?#NJDO-3km-AE1nR!zY}#8Kd(opqgdIprQ`ifi9Zd|YzSL8T?NUg9K(0-S zu1l|jUIf4O@)x)fLk>iTx+(~59Zw*BxU(ayzDOn9q3XaX-PxOmE`jbvt%&rAKV7pv zPnnJd6?d^#pK2^BOF&KAbRZi|-~B=fw1{0pt%if{h&L(tkMp)PnGeNA+Rltpv-(H@ zi>0~wr7s$T@mIc-b~#oR#2x%Y#@y!Ts@PJ@u=vLvDY7nP?tz>om+$nXH=i+pk7eybGu;-%U6UuNZBd@@J1vV34Np(#%XiCRFi|jcWY7 zBj;=m?ftYayD+rc?xvQLJ^%%-mmbwi+rRy6Ful#HbhjKTJ9~~PjDjY{cyf@ zxQC_ukTw^-A!)_7Iy7^;Ui`BrlL()10ySdljbZK1ZfNZMJY$Dd622;7Y;&lzF9pSwBAr?_g;86+Lsri(*2+G-zp%W zsMS?mA~nL>vt~gE>wj86QrUobL`2nz;dPOb29)_LTd}%Zbp;% zm8yuWNuUVU+x5ESiFnr~nT{7{TkGhzkqCvdldYR3mwDf7Q(Z3d37*8MdU zH+QSFH)bD)cun~5sxoyo#$MD_fP^0W(<_x~YO|SY@#*TsR6IX8}5Y|MEMPqLRn?wJBjeGRY$^Gb+U3C_Q-e-A?a?!0MBDWMN@p>&I2WsviuRYKG|chay=q6l!YJhb0{_@m=rTHgSh< z;-!lkTJDS8U%ock+^f<`-`8CZ6ag zV6@$^^-R`mCH3kT+qXZyvHB*bSlokn636eRSywXZ==%_^Fvy}(kXGK6Hg04GOoJjC zg`uW8Kv(WIiM4uoIAy=zv+Axtnimk$IOM{4B|?&WB2aE!z@-juI)zfvQG|V zI1}Z3uGSD%qX#f@?^0dwMBO$C%OUlOkreKmWl2~yeuwjUx41Xo9p^7UjpMX;b+&Z^U1flmymk+zq zrLQ%b&#`Q}7fAEg55CUjZJ;nh0d>{;zt2K&64FH)EPi%ZcT;afW^b5fhJ8ajzN^8F zw7EpPpRChR6pl!hRnwILjMqAJ%aATEbQu`m88%&0J(}XplYerS)Tu0q5UtY$Vjq^Y zE!WydOjKc?`2qtc&jl^#OlR42c+J%%^}=X;!&fcpS60&oqkT1>rGM1 zqalX{kBA`6?GC2ZnWsYi<%di8`HH{YN}_eTMj*0t8p)`lX1^3qNJeIRqFWQKWp`DN zk$rMqy|^)D5cEN~Kx>uW+Ii_7i~mZh)ccFo_|Zw{1F{e9y6>YpbFPZe3@;Vc@F=V9 zEO>*5>F12_R}7-2U_dIy`hLZs;Gs$YUr`$O*+JoO8YkdT z#m2IiK)MtfMZkl41LCsQgkWOA%&EKiaI$C4@peZ7l|cBhgw);NK%z>cn}H2%SFid^ z7wlQGVUY1gb{D8h%%2BzRthRX~e8B-IQuhg+a8c-noU_9ebEUwLqV5No) z`k-1+`IIe2O8njIRx1#3*$;1|=Ba@jRIY1NeE;{?JbJIfXgJ-1+aSv;8~7ZRa4vjd z!PgXZaCeG}`l3&!S3^)LORHE2|MdzH(@@N`vAO{__j#G%0dMHL2O2u+yd>819?V+H zCb!cc!MNaSiU=!b*O{SP+N$_|4fK+HI<@NU-X*LG<5Ww!uAtw{?h0;db(I6ha81_} z^}3Dg*hN^aCG0{&J~*=!*QbF>{XJdzn&@T$nll1WwVL~BFh`d-LMP;VP0m>9v*WKLayKb_-z-Nj_rROh2`fO(%yLopN}>0K5&ce?j!^{|n<&T#-! zWr}*zfk$|zv%L|bbo$X?NC^u-jx0I*S4S8EAoNFsNio_P3Gzp;&md=0$^FzTZ~Ce4 z=d?4mD9Yb9|BKYO~j;q%LM*lM=Dtm6dY6MKHeP)c}|VqUiE^#%DiuVUw1p+ z-u#%nL{9;6qT=#mCKudc1 zQ;DXpb$|1f4)>Si#ulC-a*L7?{$GOnMk?j>W}fXKf8u8wJ#3$yv+)irr1a!xU*zh3 z$pd3+DwiC}b1Y6`Mq?xJt-Cti?Qed*jURA-hxq{tz3HJ|O6w4Yrn)|CbnAP>RCi7A zbAL2#S%vQ_UZ3Ms5;0C)TVZXJSo|wGB1vWRhp<3Gp91e%9ca&V_A#Z!>KqbhEHaF}dnH^cNQb;b1cxNOz zR;H`&@PQ=E@Mof@#x%cg8a(1NL2YkLAcCESnq5<;-vXcC<=NTudUeO8V(Lv|Z$zT` z-&9wysW&Vu>XXY8f8EA)J1y^r+j=fb8K*-&l(=}|yh?0f5nUZ{+G;7rpTgG@>o*+@8#;7P}5I-(Wx*V@O zG=clZT*N1kmAJ*x%r?y~F%XuB-P1OEzX4GSF{Uk0eQ~&RniCxkY?P%7iybg%j8Dau zD_7I{yLLRP?w|2*)X^&a10srjzQ~u9Gz91>9{d+M6?*H~Q2!kAR^KermdnoYyp!f^K=i;BzbO=y& zyyBQ;UdR}~j>S7{lz;CnfBV0W%I7Wn?@eF9accyZofJe}gkd@t>p43MH<$6hM-cuz zck28-9lsztm)*iiU%q*S7o{HK4goH&5}sQ_qiheqHU6un)cXVS3e!8knA|<}quut) z7V<-=mPTR8O+ke~_Q}lt7hUrVmB4xTk)Eg(tu*>JEM!jttZ~b4tPko$E^99c^5iz$ z6$U%rT_#_sMiq~#?4Br=JX3TASo&TcPoySVrY;hoxdx~11)*k^{fnv#=6Z2dhWi_5(*bUe0_#EykviLioR%dD&398blCNSAN=z4EdgMzTz@9JP*RZ1tiwXU_|1pEyY(V;xDrUIu7<8UnZ0kygc zeho?U3b!RQDTlTFy>Smmm1Wo#=t4RCcZq8PZLIUI{+M`QGI<26q;auD$fj?LLev(J8pG-oJ{ z`9iPLn=UpxrGsyau_ieaaN6kj@L6H$#@6V*EM;)spsVC}v}CmK=#G4|Liy7X3f|rn zhuj$LbNY(f87KYx(Z!Sq7zqR#9IEJ9Sq1?lt(CgiCLKhC>lxAmYaWCab z0D7v<4|VWHnVW?_KDcDwlvFt~GMaDwBb^`0pLx;iAQAg*r1MImTPflPW28>V)x* z-%QUWsHrBZDZ;2)rKbxF7RK6(i5^N1DO9q!gSM`_BwqH+i*hG?LY}>H@mWnxQnOkM z-)u1K+>Ku*T5H)0J)CVScZYa5NgC9i+;vuZJDhb_vs~KRup1$Czn^!|Z%=UI5k^#! zOKteL(8Jc73ZvXx8K%WpA)6>sGb$L?V%~WK_}HHMuB|Rlo--<4OMm$dwNSSq5c-1= zPqrGFwX9WbxzpJJ3lO%mOt}hnI;Qt~=o^*DFM8x-FZ+g!ejFZGKzBTuRnkHqK+{ihxsJ3tr29pZ9rd zBt>OCP*z62Z$DqRoSLw#shdODY}MUTOH=$?L9x#*YuRBp|C>`v3`&;*aKk2i#Q6#U zh&dv^4dbAH4nV-8im?m4@+FPuwBMwe;L&a|8N^WNr}@Bte}D9wJl|O*xN`S%%J?L_ zj&Z&nwkn`F`>sC8ykRqxfjI(AjPwICz&gG?Ravc-IQa2Hw<*?WaJYR0KS3Rz-|4)6 zeSJL4z4a-7grL>iv;4ixOQ#&^@{+ACoH8~{1Gsn?SUcCJsqM8a-yQMV3zDd*r3(9s z)D)|kLrMe&?fRA$=Vg1i6s9Ys6Lu%$O1MXLnaUtA*W^1^EB6axFGhpc|H_GQkkDBi z(Et**&X2qQq)t*lpmU(Y{s@FmMua(PLvfFSArQjRV|BGg&0{H3(18-xz`C1lx!?9g z$8@L3E**cLlXm88k{i`?^6g0qefIsEa42rgY*_dek0R-kKZF=8k}F;5LNCOQ>K)yaPridKQwCB@g1TbmB}g_?d1B8ZoS>02SbJ9jX^7lQB;2o` zZ}X(oEq;6NJwA0Zm%XuRh%)__TU`IM`ZXkCdcsF%gd?+^BUAyHE+1Kr@J^sdliQpI z*4C6VNRD3ap-bu&NG{bx3P`B=P0Fz}zO1#brLg@N=Ho0JUOn8MA;Xk8d?q=KBo$Xy z6Wf6Ul}{V93DcN5{kHN}!SoJjFw6Pf)U|r-F>#Gl!!@D^tn;jJR#A%c{cvtJGL>je zXk|QHsz#Zd^4_E(1JtMXe2GC|a|pHlR5e9>w(WX8|0+xPX#uU^t+gv)CSg6l%d3rO zz&5q<7sLO<+FOQ2)wXZE7$B0;0s;ym4bmNg1Jd0kUDDkK-AE5zLwC1ycQY__cQ@<> zzVW>OV}IBm_DA)QS=?*Qy{_{*f9G{qF5dPFyLH=LZ-CAvx%PHgztOfhyu&Jbmrg%; zR3z%`&!g>@oII={dS^+dUgOMZzf=1P^TbovQh+et^y>Z}mNU^1i>+iD8PVm_fokM-dtb)0t(k_E5MdVRRliT^uNMeLfW6}9-zC(pt;e6VrY`8 zU55_Z7HQQcBn3B3ouBEWRGkD6=t7kz<)wF|xk4KYW45YG2DTRG2t#E> zGF_9|1X@xWKZh5o_0(h?fV%dNO3VB+FGJ70iXZmn?N@wvJd9{rG)}ShyGjlN$LpQK zDKGJ^$0GytZJA2Csuy61`T6UX7Wc0-eR~AC$ua<+_Q-&-_q3&QZOM9VUEg56NwhNt z8lDB5JC}eUTStt6-_>7xoWD>HG8au^kULjPhx943Q3&7fwpIL`C7f>4MnLk-kfD47 zT7;)z&3t!yrN+f-X5<G)ANPf`qC?KOx zV<{>}8s2f_ENGLKqJwZZ5_c~*Y(Lb1=l+Wwda@R8gZbh0)&e}1nPlAXh* zRk6A}q2+Xg7J4ujn4cb){A4PQsfyJpRT!A$DYGehyhbTd(+8%mS=Y!7Rz_V+mqB&P+_9wXS0Z+yhqEbTC*Pj_fvqb};Ncp%5v`W{h_$rhge~Jgq zzvik%ajLE{d+VZiH!Vt~3K+7B9FfrlvxW&kmY`owMG=ALe~0e@~Q*lzro`P%LxI(iU$rZJFNnSdlJ_=p9?zGg6O#*Zg&kW0&TfxH{#^ z!OT?$wLH^kMQtXw72sV5Rc z$B9O^lTJ6)C}XId(ud%;Y-{CSg1!&PE)e=lnKlLTpoe5|vI(Msr!6+l+O|WF4smpV zvxD}uS$js8j|mH3@Y#Na{2ll3=f4_EFfND6sJ6}0u-eaP{KC{c%5&#^vXbIAa_P8{ z%v9*s75zkgnz*e^++h@S2yDLd2JEo6!4O0uYNTc_AxdYx|HmV-$%pf8N`f`S+j!Y1 zDDKVVNWoFC_4BMVzEY0}k$!^768BZYXK(_Fncv0~%)Rdq^o$M0%1Z=$i!y48rvhJm zWiOh;YhPEGn>KgFJNvq2tcF}pyJccl^^r!3GWV2X`eWhtOkzak z-Ui{ine(wUUSsFRDe%XHYP$ry^xx`g$G9o zA3qr+dYfB=P(jhV$36zew;qB;C^_{3YRY977szwlX6iSze3gYg ze0XBGc3_JL3){J3?00Z*90n4a+$hjn2Q%r)ZeL>qq#qDkxaJ(DXOXC`g5zO>O+l{9 zTW@I4;OR+dv#A1Tq3ftL#&29XNE-GkK>BjWRP>5ZSV0s6M-!ViYuD;W{(+y%xHdPs zM~z6pc>HXWsNxt;;l3{ygKbv7Otu$sv)ipbFR73dBMGCs@OX=BwJ0IF+jIU4e`jm2 zVY`n1kDV-G6(wQkbL`XE(&F!h7Ml5}8`Qc|P&Sk8fhX>_`B8FL-vwu?B~|{U3f<7{ zohrA_n&H+$+wz?54=#i!f2!wzBR6ZW}gx0%$bkotrM(j}#D_ZDh{YZFrOzeyI2jOL0@B?RCu>K3!X zx$n^9Y%Mlw)5D2z&lB02Bkr<&AYG2{b>5Ih+>0C>UU>nqPQ3S8|YDs3IS)wa({O=2wm1* zxpS}DiGt+zo{NU=aBDQw7`maU>2+m31;`sZiCXXdyPFT|EwJHm%q5nU8NRa0mO-ykr#Cq3Vr<{Q=16Ls+`tEFnpf(R*?`S*wKn${~M7 zWA+}0y`_{9(*8bj<#7FuRPjN+ckHXF&vsD8B6(SnkWUrIZb$}KaW@!v9>r`PIs4Y& z8qH-*hqF?~kn+@O?~?u9Z`!k4vkki!u{JNtn}JlO505U^H7vJE3nws*E4Boi$3( z9%x{fX0q%^3l_WQJ7Y9b7{FlwsM#ZX4XXMYaHj#9tz*$0DiMcz|aq)-c59+jBbXHl;lW}d3vKnXy z+pL9fd37G&7+!Uk)37JGJ6UII3k6zy%iFD6#@_SdFIwo#=g5FiZX=i&r2B<2VS|Em zck4J%C)qC9M?JXhu^fX|^-d{yrQgKqUmV+MzMtGHhx_9I;4Bh*SMRn5P(#owifl$2 ziC_DExQ0Mo_%uPl9b?E{zsoR(&aTm)iDConN!f+Ib_R}a&^D)DXGVcK_a!(#s{+Yo znZ9>OfBtO+FH^P6tT-4?kG1_I)|u$m)nROs{^`87OrTa6MYgCT-6j$st#3`=_C5Oj~I|T7W+b}Y00r8%zclbLnA4-z_Y(RCs;YXmd z-~!TfVUSHu$PjkYhJhBn$ONUAn|)|&o?4$^w!M6Ge& z_@G(q57&4FWL*Qt#;(&3bHT>b&l>3EhhI~%UynS64&f)IiDJ z95VJ&!!IXiSnnx~*`~r}4%MJ4`+=v3`y9P7rl}48hchyt>!}1BW9DmgiWlq_t>NF? zygFW;b+CT)A58;e79R>J7Y&#W3Q_#=;KG^j>bKm$N!%PvFJL?sqa)p#a%2bJbtM&0 z$sy~HqzQc;G>=IpXg9J7C@e6|rSoQ|RW;^zFpRtTeJDs9qK`%0=w{n)6o6&6@q~ri z+Ve*@wFPs`3dp7{AsK?S@0_z z&zD^Gq@k7lCTlHaG$0rI$oEiHRtj;?hod3m^r0IP3f#bS^+sZi$W|>tkf!6 zrad5RH$>Li&`SHFbz+v`YWNQ>Ugw=JSgE(JkIv88(jiMy>B8q7OCfvurn&1%J2d+S z4d9W?ucl($nDG!*ld3Vx@&jM*W-VROo}c3{3p?VM>=^%` zGc`4k+H4H%A&jOiC#oxzu+NDEW9EZ?$zNXQ%gO`iz$Oa>P`T1|>VpUSG955%!@(|& z*)iE_RICP;!MQ%s3MS7W7B`n}#3dcs2V>t@_Du@-CC9K;ap(%+uuXf)EmZQqt&wFQ zJ+9u>vL)pmCk(GZu2^nSW@2kQKX6qBU95nqP>UMZWSCehE!ycWo>{n;A7hh#d z&|g{tr+?y{t`~)Z$~M;?L@g$TVfep3ylK{j(JW4vtbGX~ygY;} zQ|gZ3DWJ+6Y3|>dLi)qZgK4f%q0p6E?x^0QCF$;Bx7~Obl{2gDQ_(U{4!kQ}k+f^B z=oz>PwL9}%>nXgt?0pXo*H7ydaCrJ^9cZ}MFOT)ehJwChrQbW~;@EuJ3;wpm5NO(y z2XS*ryNgO|adj-UMdsT?@|;|P=BS3@(w7c&w{gw=1pdA>sj22x$S#rXWTI7SIk~Ym z$wk+5#QCPp9Jnz)zyrSoRfguH3mHIrM0V5MEN-kqs|~lc9*bAUGPoagg}0j^8i) ztK)Ui>0L&%L3K?2Qex+OAPW~8!MDCr1&{+GOrJqMC)N2e&%UO((xj>SnI#@I^ULZA zJmRKNiWqx4YnH3h{X(S6JCzPwQnOm=q9hpCy;;7O$vf;Hd2Oy9avvq&9 z6vYqR+OitDHSGm6w!8SL!LT*{YCF4^RljaobCdR0a75#3+K@eyk@hln@kdoHybs$E ze`1Db;@kcT_>3WnLx;BXVU(8F{)P16ZA4H(dIfnNRo?`vcvQce*GeiS4O~H^%|Wae zFE{#*dV|BhIW~)JMvzW7;Yt#PvV8Q&)9mVKCQNx-~)$>01{$CGHaB^b`*z06`}ZLpvZV^r{eG}_Q+@UXs@WGwe)HQkkz@Y zJ*s(-JF6A?>j-~4fgZ$b!?Qfxfg~6mk6(xH1f>fN!LUmrw&4BF=+iq`WH+aKIl?;~ zc{#>UtWFC2#1jRRVaCgjZIV!fmeBC>5aWS{)-l-jN2;hXAzJ&eEP z74m7XiM(_!#o5wY*!R-}A5ZwfepdJ}Q2xsN zCWtX&S*9Q=~w?lvwJZ)4sU6<`QdiO{F);W+0i*R*m7y49@eGIir1H zqj*-6LUYIHX{R==`bFli>8o)we*Z7eHyWl~kCH8834ClMKJep!mMr|78A9k(|De60 z2mPQ`-ojyUW$Ng#16%U%Ybs;pgJAQa?%s{x5k^k4wHf?n$;)-8*sv=-adh$+r}%N= z+Xd3$d?_xMqmyZ0wyhU7$aD*kz2pFfmD`Jznepwo^W>)L@~+Vkb3HoQR2cBFl*|H> z;ZIF-k!mMVc7;T4T#h!nBin3iE=o$_@#N)-JI}gKhOVi)V?S zOz3d7P=O)3OwY!!sZt=hDXvu6nQ8$c7OwGcHWY7C?{@0(T>Z+dodb3^=0()&Ce4-= zp+*w}f#<|HOG5PjlC{{$c%pe(Zz?`+;x>++#3vxTiioI{@lz|`En_9_nbv!0vD-nP zCGIV;m=AkYp7@Z0cs1rWr8h1jXZ(NM=kb4B-4#C}y@}6f=I@{sb9kG@(pvA4(ue2m z{r>e}I?laDY<|bH5khz=b=x(*8RhHIz&wAm^h{7kT>v>2+nz*2Viv!-$T{Qv*%}Ac z=~@KN_QUT~`t(1))5k1GJwf|jh*WuX(~o&r9=Pt_*iK||kk@NmjBD>-e&odL4!Ti# zVznt^=WaW?QE%@T{zU)hv3u@n^?ggzS|>$y6gyW2#AOXJSm~4X&#pU7I5YRGA&&UA zX>ih}uSj{{2YAJS{Wr@n=1GyA`=Faj3&t z;zOaUd0)rRwd~qQAVtkN+@$)$A#oxxJ}qb1hfG31+=_VbjWE`AZ9uCoI@zr)JU_=2 z267@kZ&Tfm?H}`y(ZI@AEG+5KrlJV79tJ6h+k152PdOxgE4@#a8xYVjvu~=0w({w-du3t z`z1uMX)ORIn5^>-4xvE3Cfb7HhcSr&2?F-)1su9R|IjgSO2eiX)$JzKK8<{Kd?u@- zFWlK!H!N;DUBUU^3t^iFD{le!iu|x50XE9%(GsT^Qv)Y<)>zA?U zB~LvNmy{upkWpu{Pa!x!e4&bOe=M^S(J$ zyISjiew5cC>%40WLlYC(tfl?Hg|ASAS4ohT2=?ijL;ct!cW zPgud@*gAvP%fARJsE}C|4I2LU0=|EibXGj}`?Np03F8-bSr~ci@6jLBnB1?`+Ho}v z7+11i=T=qkSOX^tx0xn$rrQ?%PY`&VHASSw49fl08@5*t$J2jp6Szs`lF-ICAAvoQ zgW(g2Cwp*+K4?LMh2t1r1hMmvcAj72(?$2(o9olCGh~Hm;u)rv(+`}m$8OS)u_|i8 zQdm*clOoU8Rtugr7$~0X=mFf%i~rs&Io5F>khWD|?CYyow)P5RtlTB|!6Fh^!{O`s z$_4isxR+ZwW<6OHTl1f3>K!S#s}>>(4;;-!I%&TVaxJ5UYw3Rfq_A?;T#A1+`K+Y3 zM%6J1C!x~-1=P6HSr0zSeJpUj?|=d~UM1_!NgA=&3dcn?C&%UpLApk$NqyRz%A+_I z$h^7&O;n`p9O?}%q^|ME7-q|FOWb4Wd)pLr9^6{c;_P(#z@aTgl`G(c|SMXzCXH;cIRT=TrU>-~GZ zZDVsfImNwpv=^YJ_w&Gear);|iBfzb5`sc9j-X0jKf(dHwI_ zssh<%d4S(g&t%nH*(us>?~ib}BDl%4@ejaMDd_ZoB*&HdVTm4ex4AEks;-B7ITS8c zRg_F~`;I{vI$ysip7VI8_Yf-^4C<*)7OgK_qu}firZH&<-zo1J-RE9D1>Qq4-c)$3 zivRU23cd+NC3_oWLV9?#u`$lJl*W0b7qd&VgidDjF50A<3$zXG&A+(~^=Vfm-{!3$ z178FI90bFw#~11U(py*8i;yroq;Crwd!mp3{3`MXIJe7j{;o0gp-FEU2sG(S>?8`{ zUz>L(c@&HqkRsy6 zVJ~Pak;`V(xx6Qu-4yk`9zH@w^gHWO1M z>f&Z$VpW%q8+CJ7&m$#!Jbpq9;M)|@K$aMuS453)iQ@yqx;C%=@gLvxJd~_6Wj7ta zi+d9P?`K+28o{bp&W-g=YrzHvJpbdPorqDc1_V8(%&A|>$Sbm8H=O#hurS$gySk{G zE$0AH9ucFnbq$n2rNc~(aKLc@Hf3_ze*t!I3zjJV=ITV?SWl4o>l<3i~O(uVC7u1aBVxa zu4?}z(ZA}5U)NhB%ylis8R^4Zj~k~w%67vxu>+%hWR-2s%Iy0VoxI9~dWWKg*1C9_ zf+^o%Qo1YU@Z%~?m!G+V*PZKUv_|sk}>emV1d%ULFaJ84oCF zIYX~L#*A{$l9bVqc?`tNEXDJZuPLQvK#fSTlg#1R*0!i#BWZC>Sfx-#>T+|Vf4!@< zc5)gIO)5lIVbVx!ujvC;9${^ry4%J_LHHZ^huh*~Bi~N!`U}2CWuo1MO0oGe*AH>- zwo^YXiOD$(dmnp>r@ohxXSPXJb;<-m*dN2Mk-FFqFv%lf=-=ndAbBrG{z=#EV1@W&%wTtsr{2*?-h(%{Xg3zBFDwfj<> zlQguXyV~0P&WrbUJ)gvZ2kKmw$eALaL*00gmz8~R>zcSEYcg&a3v1h(xk>l}H zVEo0{*g8ROrQx_F?H8ECeu!dKJ@tFR$^~ByT8OWD?L_OHy<$hA5b8xvxLExhWL!Q( zE!J)u9+tfR2o0v)JM2(LOH5TWpE|(I+?$U7R*%F1cmMWYxPSTP#3Qgdg2K;hWR7rd z9(YdSXrLkRScY!h@HISgfw?5~dArPJ*OztZ+MDTh&Oqoor#=1WhQ%^N8rtQ`O68uG zlon%PS;>8px!;oNA{Lsh1J^_e-kN$21jP!2l70PMu(dJpwtlgt!+jqMKdMiQp!7Vr zcE{=ajq3;Sr$h#qz-G_-cRei%YSqq;+(Aey?Kz5zVxR2uN`=C414}OG1I+JJiZtpI zjOX(|e^{eKUXu~|V)3rwG}~@vSLhd57dXqLYfOF0Q`*CCVTLfZopzK^Om zzrC|V7YAd(5)Bfi$A#jN(;gG_?m7DL!L{ymQ-p8pk}(O$Mtb(@O^^R50&6yYFW zJ;zwlJKQ!ZGE% z>HINi72>-$lqH5r!(f*$W>W@uHdCXTFk;-J)v=P^SXS{`1n)CWWwn=M6ZtjXuD2Jd z>DOqxnn$~DrO^YKKVzF%d%Z-L#n=rp^P9&%xF(d_)q(T&4t6C5jZnCn`F&SRQUMW@tDOD<)(a{|IjdIf?fMLMDy*0PZ;{d&d zm%U}vVLBMm7+xq#9(tuw?Vl=zzV=ZO zo*>Ud_eFQ`H%v?9a>^ zzv)f@n-a3T?9K}HzkT1v3-~$eR}X#;o09xE*V$Y!15{FbP%z$oi_@>^@Aq9i9H!tKwKwL}voVnh8@;t@h`H{Zc?H;$^bN##PN z_1+-N1wZx_d%jRdREN{A>`KzpzCLfXZ>Hj;->lus90^3Cn; z6sLr3bGKQGH1}Z%Z46Iu?+|crZ^?W|ONST{YIoctcnjQcTGnWx7^&`=-?G4JcJlwKje9K`^b^i>Zv9_-jeZtjBz){HyR<;+O^!L<_ySH-7 z=*cD5Vl$c!LENP=kZmI|9-!CbV{E5A=rn4!WI$#}YGF=?%qqAa;`mJB%1BF$YIdYE z@SwBFdWyh(&qdG98DxoU+^k$Czimnb3sQV0>9mv9GUb|(=D8Bx`{Xw8KtNV(#sLVV zt~+AG6KSoOjp^?3$g(UF^>-~z{-axz5OOlnuTEzyqqY;M83}R}F^Ap}zP_NbU1RfTTQFK`IZDa#u zWolGWC%8oBjXjb)cd}f^bsBh?^Z@2(w>zguuE)V9Y|PlZ0y5zfycUb2X`K(Co$UOS4a-y zBU6_5y12aHIS0=B-JHG%bhni5uo92v1gw$VNh(g&=}XIzLvNLeNLzormHAZzBP=YD z2P_grDHRF%~G_rxei%?RdY` zi^N%f$ow*wsKVdTglpe8;~NUI1^RsDW*2I=RqUfcZlluXJGx;Ovn?=T{d@;z&SN@P zKypEh+m|kuCp1=NbbL~WzF~*%c^|Fgtjg`}-RBT7u(*2vZr${1I=;;Tg;TtRmUHwD zNGFg#)LJ&bx^~;x6pMf`+XrCs@r<%YsoPuvSWugCZ8OH*-R-wK1@rYZ_na@kFn;hD zC*zAf0gqZH3DaI-Q#ctM;Ojo^1MFvL!QoBZeB^(&!o30pl;MTH0BIEzSj?REtFPH| z#;1{wFl7uo$+0CQ|D5d1ADB;Z>PAuYrKgV^#(b|}MPWV|J77b$xJZ4NlpN{bdC7I& zQoBgtrtbrzG`^}WtEqN#b*3{hFPE_NcQ-|njLxAbkZK1Uj-{j2{R*B|Dc^r;ad2=? zKA04BwGH1ENi~U6&QN^Pu#tE~4Mln~?q=ovtH9vy<4nhpVd>#-7`UcOJ)`GV_Saj& zN9a0&X3@7{S>vg_rLH8f^?Cl7ljAmzW>k%WUIuS(3%UKxraNotriO{z6Crt*D>VWeYT{J^zw>F1MF4kYb2L6(M@$4^qWo&u^CO;Nh;M0!kSZl0rSOWF6X)D$V3Whc+~&pZL% zkFRSc@2`?>iOIhIpju$~V^jn&%`@DWYKrz4_bP+lZYZ|293d+fpf;#VJXPDyrQr!^ z^3WQumjxtVl*p!w%JdnWA|c+7gelQ&(IXjDbO-R$J50AT(_D*#wKt>q@F%wPKA=1# z+-PmeH)@*GVR8JHL2c|W^G#vdU5G!#+OvtD!m-+qGf&=gs5k$V1+GB||6fw^dlTdT z^x)k*I7S7_51Bh8Ld;Cso7)~FCZ{}r(M{}AocIXCN?&aWO`XQ7vk6)9ZJ?KfdI}6q zD$l?=AzV+k4->H&P8XrY+3BT|lD9Yy*Dv>&Fh}*kEZh@B55cKB zmQ_;;PrZ8z%g#nLL1l?h&Vq?t^Sir+hgW|P{8mgC8ehv_QnLQfgw_=^YZt>KDR_>- zw##N|ONr-iXg@L>ku}IiE6=+65ZrR$bd*gI?N6;%&Xjy@iFZ`gg96_sKZY6XTW^ZhGe0gk+=rXreX#TH&A#~TJ))7Sd?Bex0C-z zG18&Ma}f9JF!n^M$jQ%PCH>B!IuZ{N7=9Y2ZF3gjr(SYfXTlq=&aExFmmP3)v+lLq zU#fYN>5d3DA;$)^qXD4J#ncw>g(~!OgdGHcBZx$9)h7rlgI_>o7BT8BYor{FG9`VB z6&k@;Hx07yFiM@B^zPO|yZ0;rZNwX$B1y{k9#xTpaPTwsL$kk7=ad{6ML>KP9*CX^ zqo}N(z=27tzb3a!LtBOH9BucD6!}S}BL9tWS$=$HPy`-naQ8p+^o+1y>wFD=w-LUA zoJcKhodZPKlU2DHV6F;&>KBX7!g7Lm@ST4IU6waUM4ypFgAIcq+ zM^RW}#d~^EcGbD*>LE_1)uul(izs}hSDWqQZ56}q7`6X(Du_h?Iu$u(_9oL4wDOst ze|aa?3Ni!U4e4a$2(GCS<61{D3f_YE)TVc#>o&+IXdbb(CSPrp0eVzmRsI9;Eh~4# z1Xi8x0IE(HkPke;Xq?}Pjm!q-iV4`~f2_~1-t%Rb(52=Ax?!h576YU95*0)d@5Y6} z&`PKuVm0?{=N_-lmDTEBIWW;6AJRCm)rT&q#eBjL3|{F0og7lWlnclD5I(H1H29B>IR%ybA@WXJ zo?29PDw0uw>!Lz9oVf>k!RnkBpPXuYk^QC@XBq=Xt$u`gGVfN&vs)|b0)T)i)q2)~ z2r1C&aE(7WjJU;k)7lz};+a$!xS|#VOqynLR(puCEs2b;JjB@A<_`Jjw1ibDaRtL` zC*!!cUM0n?vyVzcG;0obS_6e{3DiPf!BHoc%E;6IM!b=+F_RYWe77qh`>p73inJiK zfF~Qtbj$W!81#9H?~2>*srBN3h1(65u&-rNdNiB;igFvO)-$wJ*cM=&M0~;t5MR4S zoKbc^Al^Ei9{$vlCckeIEr_$&W7?%u9*KSrZ;!;@DI54)48`cmG$&d=1w|X5k{Xl0 za_|ISc;oZEAv|VAt*tx@sKtNcquXVPh$ZqK@fHpHAr1)nYpV6D=fv*Y*;)?;{=^Q5B$Mn)rt!)x*MTMI2Tz~WYsK=-?Wbl==f5j z8XjHH&t#(ZM(3#--l^64NAT37;u~Do*TICE&C*vN&QsfE*5=eH-acs{JKo!YgD1SV zX#@14T`7|wT((4yfF$5UEiF@=Ou&b3Jw0}9?*LL3a87lM*F zJK2b=?GHek`-N`NB4-P(kd83I*{s85&M=;5>!EPxgrq$X=#u0y7@WWcZN!2Fr)RxN zE{#}9im-8pt*4wl|R(Z==tul;;2Z$)ASRMt~x*K|@+yHS)*GA_RAF$rzy2Fzl0 z$$~z{O`}s&YEMr~IUqlU{SeQh==yIw>#Jrm(CJIwn>M=^Uq&QSlK)xJ<@*Se0h>>I zNAqXN_C(>@g$HmA7&63k%%eyNoIH4W(m5}4V_VOl)~A;Vwm}CQ=R*)(VI(AD*{I$v zC9%=F5BF^*`~q%vj!vHIln672LMsmguRxV44?-!ZN{LN@ZvJO1m_K=tELssK2^yx`Ei4NOcqflL# z|D?>Iy>qnZFbmQ@&@ejYvxi5%#+tGK@(`)wIlWS0iMNn?FT;_G!YhpvFng@AX_!&Y zuc=CJWF2q3mN*%u#Ifeq1z2MJ)2sJvr8r8i+yLrkiQs4rX=;{8_J3GHqK75qS(P4? zIeAcBX}FKUGtGKR_b(U$Jq-g%9^bqXleHn^vJ&FLA|~^Rg@&p+2yhDYA-Pl!2vl|p zVIPyq3wxs(jx+Xh%(F#YYv*qfa=5rk`q_h%5AR~hO*=P=mvfyIoaZOp@4v~}(QX+5 zfk3UtKvekZ-#`?FlCd2kiD%NqNd7;QRZiUU-f30D4g~B6$B`0Nj4i_E35_e{QO4qi`S0G&H z#nNFP$wbdx{|5rYyM`qNHP0)S8w2Y{{Xn1xUW{H;`6}wWGDYa5GH0Gbe}HpDXj-eI zRugj5A$;1F3JP8)9nH+_Nv?iPv(d@cc8b|u0z}h!klY%v`gIC}C8F6!M#{`D1lmoO zJv!2A6zK5f;$UfD@8w-Zwopx;U?YF@;SCwa%4l9z)xbRg4P)#xn+TKWq}pFb3xC0; z3d*u?pCl^|bLmK`&RUiooox2C^ocazXoepB=4*M3cnM?wjd%&L4ikvnTCHX7E&s}B z7HdTV0}BZOc46`~b*4209DmZkG}ucb@3@%wq=Z*pj82EiEwTn1K=BZnlr^r5MjTOW zwEn|AZ*s38_@`k=BqQxrvFy-^y^*bT=E0HL$c(Y-u%eB(P+YWTKa+vlzho7j>!Yk< zpJ{}-rH1*EI$HA1chmf4vh|--;K!L2g)&3R>V2XN;RFnpV!<|wcKqIrS@SBGhTCVq zqq*c$aXksonxieED!Pi1a&iVIfOBr&Eg<5C<}#v#kOi!}=S0R?%S#B)ME=A|j4?uj zWH{85jTPIcN9(*_4A+c4WW9vwAF^IJNzu>xnc(2gbU*zcce5eEdHMISva7BZv@(Ov z0fJgbB+$AP>PX3x-G_*hS~}F|7Oo0Z)47f8Z-Cr-R0|&~X!xm&MlMMs5#5~2`3Vfm zEnr*k>J?S%)#p6w6E-~tj}aVw1Fg$rQ?!!Hm0!Eo!94~#=!$9~EWN}@*ZBLySXXOK}F)Om4lB?iWe#Sn8X5GfB_bfBnA#!jDYF^{NQ=a|Ca$6 zL1PNebOx!X*Do8wqpN7WttH}FHk}~OYi;&7*w*>_`Uh)uI7T4) z7R=Zj%{2T(-?u0%cR*#c&qhmay%(r^(J*T{&gp3I&jgaPJnPCa!?NJ^R)PQrJuVzv zN)V+d>nbS{pPz$zT~(nxIdRZa1y*Wd$??ek_iSJfx80U$S~uxG57x~I_Z{b`9wZY} z{mB-LzqkL3)}LZQNmayE$X2DYXNnnlHWly6`gD!h`{l_ZEnZC|D4-?*jB2$I*kyh> zq$g20T4xf^ASz_jlsdXY@m z{jgk>UHdctiz)xxKv4Dg7jB+;*x;nC;1*%xp5_)Ux?5Rd2k+stt+FZ~M3wQHRuAW# zQNRZX36&gw@Dh3hmVHC^xXT^8tA6EA6ZtU#MD&UeA=lhC|Jvumo=Q1M9HOxEqmHN` z!b`8AX_A2tAtuPg&ORJ^Lg%FGXHna#&J30k=Vfc^{(%2NI$X$`ln{c(L`tkl=w~EyvW_TO`!8xhN4YXTi#(fmhkmv z9M^L4IUX)Eov}x*44~~An|#X^EEz*j8-U?$sgqWE*q<1*>I|yQAy~KZIL0wnb2U_8 z0&`>S)8;1sdOmKhFDiCTKVW43uYBDgp)0>r@c~bYDGWy$As{R}J(3Y%ODn>%TF~#^ zqj)MSM7_?y50p(!WD(d#7nS#Wqcco<0e#f%JBESgG{+9S*TAKfX+Wiudj)ge{y96J z4(mbJVmYlF&wJN-ed+fe980Jhv)7$<2z4&Sgn3yzLPd#<7bVQeTsTKEZ(!$+#Wx9w zd)j5F(9e>?b04%=!@e@Gi5&ZJ1hch@1>Pt;7)y(9r^jpTLhUjO& zMKc;`OwYR2lJrbELVY`Ht1*(xF3oI8r0E9)X`FMJYo%_Ei6%BlNOE#vKK<&!^t><_ z5&svpH;F&}>F46Y$2#|a`))n7!w32XSMe>nUYm_a0&r78&%AOP1V~5F>#_zD3iqr&6rF4OLC%8_>Kj0yzrbMh_92bHl@5%ViF;VzxX3K9O}IU$ zPc+}G7A~4d+_Ox|(VmW@ShuraDdn)*?FB5_5q6QzY_pWXcG;|t6~|Nt^$b^&poInu zDq=TS+yu`Z3PFC4)GODKgnC`&G&=aBD*II8cvAH7za3|3$U=fte>=|Sy9yz*YgXG# zCaoe8z|CU39irD?V%7NBe^lh}j=$;n#vR}JvC>poR;?1n?lt36gPks(R4b`$(F3Jr zyS$eK5U42@Cy}Qp7mj(TO>=?cp|50{(j2k1NdgT=CG}C_)~Oy~|EP=q!v4VgN07+_ zxwa}7V*1E))cyY90{1Zkg+`rjbr$=Cc&>!_sbPc#cUTyeM8GHU{)bOP2%TgYl*ix$)}vj)(k!P?)?{_O5W7wd7;ekEl+EE^)Wr4 zj3W~v*TKPfWl14H+tmUeMw7CR2y^UKszC@j+VHKSOur(rJ>0yrr>h1TaO%gH>s68GESXk~r!D{OP-Idg zXn%i{Ae*9)#~r=lB+J#F?9%t+yWRlof|mw$;HwAp{G?HMac+6OgEGL^F@fhN7agx# zrLnIUI>X*h(t@SQ{gW0trv4Fm#{>4K^N)#jbx%PR+?Z3hHF;gVh=)b{r#Mg6m zc#p~TH?{DC@>j+GhcP!>`+}bi1}14aNCLNuQUvf!H$0Hs3mQw_qd+d<)A$nK?`J_a%saGhGHAMb>W|jP-?;fnpZ__s0^XhiiXA_} zx&NkZiYzi>bX9^T$PR8VE!?H6%A-gQ&H=xODu9cmwY0t=tKjB#4EtZa^Xf893yiO4 zw#hV-^%?@aClz#p{xw(&?km~1Ve*w(*Cd;R}1x$FP+hy8J}Tj7tgco-Wd zqxyheZ9jRO5*7ygF6}OqwqwJ3 zk2cF@!0(?O;Wv`Kz_=|zQIbnLqi^m|vYgF^4Qkwr+wNE3s87bI<+f;i?JaJ#2bvkM zh=&HB2wMiq>1t8Sy;$-{N5z{rD)_1nrW7XIc*6q`T0ZzWd8%$V-g~DNbRH?gCys)x8{ zv=q`4nB|1{?wMej4~JBYHcx512ak)2NGI`K7cXUiP!GM_FIB$Z2+5NXRAalt*zB&% z$1*9-<%e)nn(T~3+;?@$7!?_Ni5bmE^c>fLNxLzc!W#u5nadS#jT-r(HjUYH+aJ!g zBQ7zn5B6|LTn^ch|9yR(kOWQrMD7#t3~b%ZOz-PQo)3UY5j(q7vCY^?mSE%@+pD8N z{`+uSxhRR=AaY#gMSNhWvVvE+)gy5^>-yrdgVLkyg5_kgh?g)j zosi~ZFEP+1j;;Y8{i8ixry9->xMzR7SHI7BcG(a-7s&VG?^7X{0nu2I-yk1#c->9Z z*7jDqwX$`HqWp`pUA!S-Etf~JS&1=Y9weLMiSxUp>7n>pVizd&zxX^Rb7y3NMKzx7>%w%z4Jd8ae{IrWmhS~gih z7crk`LVFZ}JgqrUeL3nvKP>MH)ACTJQLEgB}@{`(BDDj3JUY1Fq@Z}fMjMKS8%OrvCRMeLRmGd*ukw>F*s0l6;4+#! zAkm#2{qUeEOWQAcwK=Llq|O&GaEKpwg0EtSQ<2604|{I`R>#t;jV|27!rk57-Q5Wq zAXspB3GVLh?(P!Y2_D>o26s!~uI#-(Ir9DIIp_XYp6A|KFl(l&ySk@mRZn$y)jOGo zGX(kruI|y?bnJY-Q1Cc%zaSfiZAiKvN%0km&(ZbtLZ)I@uJ6OEC?u3!B3$D zd?dYGbgiJ9Kaj6Bk0h%72EtNN`|T!k2kNc=aFfa2+=t3JfBg1)ez%K+w;zkAwf4jG zxR-}f!+V9<%sZZ_(qb>~m;3G|UNH+5F>YHXRGyBmsN4DU=}S3_D^??scadL? z81MVe_E9Kt6JA{&g7kVOjawcZjczmCKE81?gtm#}=7Sb5fT*Ev8sZCm*=VnPtozqh zfG&0@AHJHcHdl&$YforG=|8~1B6%nFlugsw849{oYXydcyxe&hcqlk{DE3FfMVc9O zrA2jZwg^fu-Cw&@&AT&5U1|-_;KknkjN=xLh~)k2*I#mRTkp*~1)4(E?g|pI+wQkH zz^#LZt8fUsgw56yk8s`4oGrnKwo-UqX-v$en~w^7m?t4Ys~{D9KrO8+dK*-_#kWsN*qzPn@>h{A1#@ z4X~20$-NtvsMWjFk;#1ls!u;i+%H}eK%TlekJde(EUdEGZ zKpm6h_{M0Y&w2llDIgF>&8j25cffTUk-qsbyNJ!+TAcgj*~J@WWmB#qC4JIKz+m1Z zFciAM`g+TkeQvypu4zP|655-y#wf%ApTwqQWyl?a9zm}wA1SY(T{lB&T2s=M@lIH- zLjRO9Mcw}oBue>uNd=_tzy&uGaf=;_#x0s;9sfnYX3UiSu=4^y5T$vr)-HkP#K?5N6Im~PHY)g=d{7FXxW zOe)QDze9Y9&^tR#WlO(^k=_kjaTxI%ButeG9Xo@ZRIaNTI1o^-->=0Jw!eOUBe6*P zT>J2lx)W#@b+6`tlJfEr*QM#ae{3>==McOJsmtc(J;BWA+Rw-!x%r(344d02(5HbU<@ok!@3pkA4D#^yt4 zZpG0&EC}Jf4Uy}gvz67Hms}mZuq7ZgqEse$AszIpyrp7~c}}1gi$BX}&#YoZrhr0Y zGg?6I2!AZ5C4RJ$Nq1UeQ|KRU%um z^c!rXp?#Y~FJP_{#+p4;BMKJubMjh<&X9MCO#XO7aT92+_8IW9_bo>db*jjVf0H$Zr*g7_2JglcKmUTM47xZI&xg35`)P)w0--zQzByc^hZuCo=Gtel+%R{DTg}=YDctU~j&z4u zA91rLgTS`U4O%5qr#$Zn*ELp`hr1SjhpRqVKKanU?i7nlVJXzgJruS zlivsawx%opZcWEZ91R}zWNW$W#d(osK1ONbb0@Tk2TT|%)cLV`0TB4#nu|0uFYX;^+nd+=-s-1i9#)6 zB5DQ3pAUiv9FF}p2h0**RKBoCgJHYVO$(&x85Jj67D|Hi<$;kS8@?E~(EQTN%2piX zFLqSyM-|Hj?*e0Yj81t!@BMTY}1NbmWrhr z{_wL%{%3N{Qajcn(vp_48pwdpQ% z&%8Z^hshx^_25pjP*(jr-WK-T@wa*1L*Hl){?TFY6dK}b&=1mb zVx+qJ@-cfeT3ZTBYN-)F;@^0NWWS<8<`W?Ob`guZP5AoM?|@Q)dTuD95Awt)*$VjE z`_CdbIsvQox|^JVV?098co6r9!z;Prn)f7xnM<2e)LA25cseO zsc9ms=#KfB3kvL~oZ*M_X8}p&t}^7m?vlR3Hx-z5p# zVJ2Qu;cC$E^NO4t?Qok)*?c?QI9{5o&9HavB=uJ4z&OG9BAUhqqN>LuzbiV zp4tHg&H1mM|JJg?U-zgf#&~*1%`W}cP;K?PQZe+k)n7xC)8t?p zkIci=16UWhfV7!qtk0hnY4{$cTfyo`$V0~H6Jbq}zG<%h!t~PBRd>ga^b_B#>W|5) zAateQ!wfa~rN7=Y2Uz@H0tf$JH@f&;jei@a0va5Fw*Lxb_&*)0{E_p2fLQ#$QJ5%` zPpoX~?}=RiVEd++N~mP+ulM^sdjC0Y0?}c&E<4|MfWj7kZ@EnWuO9yWQOEyB+SK#f zn05m+><4Xr(^!9)d!WOZ%Qw_aSY>!qTN5W|M^i(a-$!;vmhkW_T*S=8zmM2Bd0069 zL1z1l%+JrH?qP4rq@`qJX=?1uq~>De{QF$S#?Z`^Nz=l_*_@b@nVFbL+SJ0#+?kk# zhn<*7)WX?G+0;?Y&c@!(*3{OSn46eM%+A`*QO(}a*px}!)YZb+R8>lpNz%gF+0>Cq zTufcu)Y#6%lu6#y)(j+(g@c`$OHdI0ugbnHzik76vXU~A05C8xxKp@O0N`y6AO?T{ z2mk#C+8{xHP_R%?kdRRDFfh=ti13Jr2=E99NbgXANbgYIAs_%TfhcI`7#J9c$XM8z z=-8;}80f!+fI)!rKtjPmLBXLTAt0gux3{-`017OiA6yFpj1&Nl0tSHs_BIH>2Y>;< zAwbpr<6iu+gMx;DgaHSG1#xjejNjWom;f+v&=tI`0uUiU8G#U>$R)4?)%phmrukaq zF4ae?=|~r|l^f=tbtk|xo~GEB&;A*H+O4P=jXM?Bb}QiZ$MFb*B#>XE{_aLzS$VvYb0DBt0y|NfgTvNPdQSl7J5D%Er$JyhpK~Y5b+ulL?e^CD6rRrC08D~f zn^DRKG;JQA1(QEiaa+D%X?S{L$va;ew>s`_ee#g)sLj!eDNIa$SI#nQe53I(*35Tnh8>d_torpuRveBz=*@-ZuW_omX$@D+? z_>;|KQ@$U2lGjoa*iz+q@Ya-aXljp)cT1^$J1DGh-yc88^*~{|#_wa2O5E$F$U4fP zy&V%2)^Er8n&N^)=$UfGGd?2el(N_L&Bto?eC6sKkGuQ*p4HDgJ|d%{+u`KG%u!xv zYmQtsR!&-_{*fODrypF(XP3CBBktwqWb4_W=u7q>M`DY*{tB;Ns#>VE}H9D0wH zKP)tk9Cf)Jj&8)ddMindm>NdRt%b{bYO$1c3Pyh6`Xa%zzAD)KD=lH*WQj>2bjS%S zH7W&Pzw>ssMV&%0)Z^JSqdoi94p;vZ*H6u>6_)43Ydeo=0lUfW`QQ`7nmhmiFT*Rt zO$4;0_^&pw_R{uJXEfq}#sB}7h!Q=>(SO|%Fdzk_1_Hn!pg1y0H+vsY1 z`!~DAZ{2^Vb=&H06gM_G!DSyg z8ah_RBt&WNsPU0}5WQ(;TzsVPt{)i$zOyW@NtcB+XBp&&3x(DqaV8NR9W$P`+8QlERg!{@$Bu7SaI>FMDJXYgI*Vwrl z3|JJuHfhQ4eL;==T)(|Kt~&A2RKJiy&P(M$j)P5Hw9NH=A_-YfyhkP($vq8PbW78| zFCt9R@~7qOOXf}C$kJMqkqPNLWG{*vboGHxQoMHIdbG!yvK{Z|YLcBRi~s4QT#`HJ z-%|cyt15L|fYMUG6#V)Z87uR>{w9#=>feg3vcrfQ2OHZ~7^3^AUUD1_HJOQfdCl2j%)2c^IW zeuQl9JNFENZ(P6?HOYmyjY_?woIe2(vHax6@9=AuE_RX$12%5}9RiC^ETZ?K;>%tU z%%?ywmdT0&2{Fn9IxM(1z=`;H1kb17cL|av4sj6Rv4%JV6xloCgW0>YJe=mpFw;Ph%)*htIlDD(P><$R z)sFwFBSHoi7e);*g#_@$&>W%>?~-Yag&QjF?X4%92(59E+UG}%kJE%)_pjoANa~$A zt&EWhCEcbU8t+8?S=8B(`UVKcn%PHYJvs*N!-?VN>uI;DNhiC!bH#7G$aYMmmW5+^ z0wD=IF>+=mR7S7|gqX9gS3UuI#&E?0~Yp%1NSnR4l z11s~zJ65;cw^+NIpEf@$%}Z_|!C&LJK0I|!wcmq(G)UO~vPrSSV&<(a83try5)IB{{62IO65lk09ReXJS zBmX;mUr;wqj{@qZ!6BhQ_T69oImkFdK_h`+QMrT!qKYUJ8}6W!`p?GqUH#EjgA6Vq zu-=_mj2Qd=-AADkBro}<8_wHISx#}0;8nFR3)U*FWJ!fa-R+s4Ts_}Ya!$6A%DPB= z9NZ;l(tg?!VecB&&=H00j-KF{mkUYAJku(r2h03Oyxr_Cxs!RKwaZGTIx6p0byY6! znyj#+%^XYqohD2QZePuNlGaW=8?=mUs^$qw{0utV48a>Q1s%DE?Zi7`iDiF2|B-pC zo|{CgG^^%4UUY_5T6WR)BvrvsdiNq~V)5!-+t#vD3O6QJc$qhO(E+yh)4L|IXFRZ* z;W|W9L>7`*+-@z7A+u69K1ljBATHa%1PlcX5ynK8L4XmG3eNI0Q^@w^H4YN&>_yA( zHaYl{XN(W_;m&`y>jAYc(;ux10R;;(RR3(=->nOQO2Tp}qO4*#3kgJX@Q=^$tKT6e zWflFSfx&+_@Y#C_90zr}$D*b%vM#ub7+N42{tRl1EQJ0q;2=o^xHP_(-!O2bGb73n zjQky);E~Q!j~k&Z+c}tZlD%?az=~t3_(Ixp<;+!ajk*_c{_Fb~4IZtdV5{U%{caWG zh~{~^0cX+ri2>RrI{a!9EhCJgN`eeTG9&a2U=^>~v+~ICkP_e7WqTQD)=YZ$j_&?d%fo`V=5C9I+76?dK2xus97|^sVV1^siEe`*jI09ptf z+3$n@$gwb3f;S+)9ZxFvyS&d=QBJy_BHWt6hU)G2vlk7l7yuS9#LqiXVb zwk__>yd6F$I|1&qQn$%$cYQUgzIBVuJ7uU+!-XNFmAwhFhg87|?EzPwRnYuwFT_E& zH|1{t=|fFN%!oeR2b7M1EJVb27#{c!aCS%~T$FRVx%BX>7t+McZozG^ifa2Ru${6z zgeI)Qo#eL2XDU;lR#3AfA?cr<;(sjsI)e~1sk$!TeaCIoDX8~N@fR6mTozg1)55iq zYs-uTODn2P3gZ@SSB!3P1cJP+qH>ORH*QOd4CI7UAbJH$vLMH*TH3AFvR|hCGo{$X zg$4tRS#uSuJTM)9G=Ji+7Rd%-!4(F)&D98DX*0x5!G3PmEClF*jPnJq+*u&>e z!OQLa>w(;8SN;Z!WX9%}6@4v2XWO&$lpja~c!u!k9Lp`Sh&Fs78Aju5g9N0VU%Qt( z{Ju6VN0p0RUzL%|1(J?Yhh7nzMO%mN3fPE=`YR(+c zAkZl_y{yDL&#>Ekfw)F*7UG+kEJu>bI+z(tIBzl1_k^aY={Q_VUPBV9wf|H6eHZ14 z`VDVQb_&ACAXhX;ljbo=7@Oh&X#ug|9Y%Hav?nUCGt-nm+I1HUqq>U@-->o9o12O% zbT~h-%PPYB!8n6j`&vl)yWU1kz(}3h1`Cnq?LsR|(|h{(mPIE7(>0=jsp14{1q;h) zx;=Zto&HXiTY4Mv1~bt&0EceJI+n~QEg3J@s2Xp3k0tZ5?wmv7SX==g{-?{zXsij2 zL6q~cce4gkhbmtk-0PL-z2euxCQ$jgA2rSB)rzdxw`N z+gHnOm1VboPLMF(G-Lg8zL30Cu74)A1B@*P!@~*!Rf2EY@*G& zjBSnKoMMAVwmy0fi6jqn*;h2|Z&dWV%`sFL`a<*?N{qtF>r+xl!WqTCwlj+x??`Lrc1M*UuP@9u=!(A><<+nDhOgZHILr-}a@t6hZatqy@zwKj zubEE3BSCaE3>fMF1xM8$}e?XGxa&kjY9&2GcR*pUYLWVo#;rpG|Uvr zNLtN6){I$)5VoG0HC75Uz+drN6l^wk+W^e0==)a%zrJ~W8F2&B#TWa& zx71ajW{9rSTfY?K84@V|TCu5%wT@7gk931U99A*m!6|R~#4^kSp;32A*Im_FKdN+x zUCq3D%K6wvKaX`A{GmUE&h~+xP#dziL23ZQi_3MyBhXm!u<~kOWyr~-Q3>5kWgSj- z+ui=E5~uTvR}~)i(uw`yc$M{Tyk0Kb*Sy16ze*!|;KtfeGyZ|vz%LH#1|7RGyGpU} z2&elfhe|J-Zal`bT1^2d_RbWC5zlA!jSFB8Ygc&@*COii_X57C`X~Fcj{y^w)ce}@ z^NrV+IQwc$Zo~UC9d}Q@OtfeCYhYVAcuRNAzqr30$)W3d6~P-K|M)(_4xxQu2;;Vn z24s15(0D%8unHqvm1&iI7NR{}zIR&RV>L6VP0TfeZ;v?mYHM?lHr?8$Ug$j}V;*NA z*|tTPTlur+p}fY~#@2Xt%Hj8=yZyd&yqwyRWHHDVziD{bxov4lnB!_*Ha~tJjQN|I z+-&EcC#8paLcGUMOG;B6J}&=}n7j%N`&vmT`JXcXiHLxyREPVAn7Y}IKNbFK!T*%^ zuPQ9MK6?MF>L|HbF3t`oV2@@uu7dl>MFTbD6Zg9=wOR8(#+ zVwl4N=}mhTr7TA8P&|kt0u3)|AieMgNQ*zKS@YM|<5adCNnlmf%tbyZJN@KU(&MG( zsD~qNb!3GQWpY{b6|3RHL(I%^nnMXmr8E`)5X%QSTI&)97&~8 zq_-nHSRQRcZGCX&BbqQ%Y!?%B!r65jMtV&w$uIHS67_NsRhyLc#OFC%wdD<+DkCm` zKQ?p*993#mOt7r02%JvAOWcQ|2d_4u*6SjzEZ(_u_f8XqE_RBJ!`UL6YYm&YdE1PY zE&`k|>{hB7Mjo>+;lu;+WMzLK#CUgEHPkX{FODxLbT$u78Tb)SrtmagLfI9Or6Z z)87SzH?c06^8Nbt?sGR@JIW!6-%k8xWdnRKWO(txu9P4ScpSI@0>g%iJ))CdIx!V% zD8m8uhqZI4rIrawBoVBG>{i|VG2d>z;C0=!@2C#LiGI{pGGy^_4ejyvzVlNlyAE`i zad}$H?}WVt*=#NOrAEaSFEv|AqsQ0xT26mWwLR~K%On2pZ2RjG7w zM?|M_xhIHL2siFgtJTRVqwriSo)tLAW?v8wa;bNa~U{o{?_M;Wl=kU(S z7;6|Ijxt^ukGYEb^gi4f<$La9nGS;vxq_CDaX8UxVeDfXJW(&6Y;B$Tcp-$nAIwxu zQ-$T6u*#G38{k$QFA~;oAcddf@Io0QnqY4N(`7oA7FKF!+__<%sctt?AKhTUCN)yz=ROW0%{g!#qfhmy2VJ2cE8FTQgN539o)HDlz$R z;HiS(DMzn!1Fq^&_UsyC)e0w_kSn|hDycAA9C;%Cm!%9N!=bwv)JDuMw>r_?jS&2q zoPbvV-h=SwmIPh>8{nqr-Z#}gfVzex8^a{TUbdCqafttB%)$&$q|P{j%3K?oxebS7 zrFJERWPO(=?K>TwNtR#r2)?)FSYc~xbI7926;wxDad#>CLb(M;(->=*pGn*Ya?fTc zp+x_y^6y?&ddIuK2}ksE-Y9Q}ZGIZl_MEwcV+I|_c{|F-@ogGAO$h!U*ewh-NbQtx zl=4kNj6$AfPUXvciMy6^_>A-84T(<;U__hwKdJS|TF*#I>96+aQMZIr-{b0Z1w39HnFmBi?Q>uri zM{1-EGZMQRFRpR0L%Jf@kQ-4xfiuP9O*=+5gSn&yh(U$s7xR+fh>PIfW_yTYbjLTw ze|oSiv;3FvsI5= z-pI?{_<|k0w%e2^ zE#|o469mGH);~@u<0t+y*KKkbE#%b>#h7mBIXUaOdjnkMm%tLt?3)rFUMNt*@B1Ux za5Hnz8?i?d1nzjVyaCQctDs5sAc*)ux`fD$G8{B}Ni}CG2pwrHj-T}d=M9jN@&QTJ za(XdQLjVe%MU+!K83u^&Sdt!E(xoCr|N7N&O~M6ddyH9VIflI)(P(oRvAT=cSoVabw^Jr-<7b)~txd*R($ zhMB5!3rl@Otpwf~+iYqJZD$n@{2e~3B#hdhj`FQs5AFD*PKPWuY z=TsqzOyiED$4F^8kvZEWonrFB^4_wINdDp#WfdrZ8y6QDNR1rhy z$1v-7#B*e0j}rzw_Oo`&V3+S7@6ALnl!2H3*l0_voeDbgghL>b<7Nmm2VMi z6R_(DjetuDMI~IPD&;2@Br}Du$v~X(6KUfD`!B=0TP4646vX}RffGjsK~LKUm_+e( za%%kjAt}}c0K_^oF07B61A6; zm)RQy?wYtf5UoSG?aK|f+6nLri=fTr;r zo-!Mpk8d#QWpbvZ>XHNX<40=%v-89mIvH4A5;y>xqiGsk1w$7>Q(Clq5yZhv`2K2W+)yh*!W64gayx3(%@F^7+K$DP>`O=jTdo?sLm& zSA}QFqR_xB#6t1XtvcizM)nb@lR`Cd(*!UmZfWh#8nHl(ZoAd?Bs8^?Qmp`o_R=liu=B%4i0F!Xa2KnIpV`A8kzPgoIf1`nje*xP3!W|*qyTJ z$qloJV++euG%aa`yfekt`RJYY7V(5zq(zE?k~@O_4?cijsC1vOUvxkbtIJ)Qzjb}Y zOAM{B=mLATVbXo9?6Miw19E2sJOPd+bT*{R5H=W<#8XO;v|*+UVt&;g@9N+@7a zjUEcJDw~)Pd`mV&(k?5r&O)Q3pGJ>K;+gF+G7O^sv9hd#`g4{R1U%$=LR=s;gF5?_ z;ANB55^TIGA}jnIF3htq+(*gi;8y84af}EM2HFsg(_m7voSUNgA`0|0fBQL6Bx?1kz-;Q*YwNhU+d00M^A48&$) zI2-=Ti1lfL2U*!*(A(y?V$OtPu(X&gMVzvTP`!kZke(xpAEP`{Y1XCY*8(yXE3`vn z{M>ob@5u+}I;bJYNa^FhF%t3ZsZ*Px%IS#UnX&mq`!OVjog;$T5bMQCyHLvtbH=p> z*RJBmo7B(H$XE2IFlJWJLL>Cr!Qrkaf&;)l280-52SKPYH`i7J!NZRzR}dl&uhO)~ zQ^fp`Qb^K*aA@%1>y)MY9nngp?^d~fnuO&#JkJ4X(8*0rLwxP0NeU$B^@2DJ`Ql4p ziQy+#BtIMcVjfKk8nuB;tEo$uWcG{qiWrToE=nOtLQ_)_iNhiHhai)N659fST0)!T zCDUNfSbU=PkHB^)Cr)muvz#y}Thu3r;x}Ky`#egIEg6pxO7xvDh;x}9&Ni-(kSFTB zKUua2@Kgkql#?C{s^p8XHJBehU{-kSg}EOXG8Z%xpv4>jP6G%KJsqE|C8LRj1($Yc zFO#4TsDK5y4{~FZAP2y`Hl^Y@2^Y+~{|qhmC0^cQCcVy2&L*@AZj(avT`@S;8YBLw zBQwMzX8#P%$vKo~irCK(4t2`}fh0mTANX;3j}%PmH6ur$$sZ#O=mHH!GNKHA zO5CzMS?B*HmYKZFkqf;Z4-*fJnto;weeKiPB0TIEQLqY}3rux&10t#PJ4CE?czE6! zK?lYoHgj0}c?GlZL}=N$3}VjY+f*_ta0Mq$M=@~J`-E*_Oq@*K0Y6e2U@y3cvy74$ zc{n{e90eq?r}HUpVR7CJheMyK&Vo6tpEYzHWjJk$;VLDDSXmlWeSjR&Mb&2kKbeL# zdakHkW-sDXBr@f2VjmLvJQ?~-@&w-y)j4QITHu;p(nbA1EuGC_Ye)djkB_8K+tw_$4GZ9YDWF)SMYHQU9MH39fy7R{Ol-@YTqZ5A zJV65*-5W@4B$|iK8|F(M9uM$UnZ;oa=_d}{RP#e;gH|PNgqWF{OotT4Sr-PoFj=Qv zr2%3ttqE+i15zL#NXmO-38n9!{C~B8Fvc&< zgqg3z)8$1#bV7Af4;I#8p z;1g?{c$S#i$2d$bsG7u|osOk5Yp)(WOFRF&{(V8#1pB!RulAhQ7~z zVsKir{K7xr=8#)vgW>(MohO(C9{8WH^g-i9!Ms_*)|YCoefq=i44jEJT^I6TA2XZC zHqoP1)A(Q~?Zp~q`Xm@HU}2QDK&I~hsSjx7_kZ^3`JZL`|I7Oq?frjudxAztR45=9 zDhO~WsK0wW{xw1}bcoNdSN89lz1+DX{$qgjdjTJ4@R8y}FN?7rbxn5&>EQ_n2YWty zeB>ryse8ex&;03+_j6)e4_ZR2%I&ca$Hy9Ir56cRl|^@ON|tS0iivk}nCv+N*UAYt zOp^SoVZ(DTXt%3Xl_kD*{RLMwT|(1XL)3nA-p*r-zj?U7?21JxYio1R^&lAF(5CY( zPBh-n3$^aZfVX3NdDpAFta|H5mK_U(`|vIp$3C3RMGjj1JWk}Bb-?2bTsBMoF=HFE z_qyqycDTA}BM;**K-0$KoLgEbIyzn|{Zh{Mg0_1Xf@fCu%M8lY<2Cdv3YMyQL|1!| zl=w=iD4d9)cf0LPZ9m;*1i;*Ys?}=-n=i9Axmaifxl|SjdxTMtvV>SSedMf|B4|?A zA=aJBGr^2zVIj1ccHFL|QBVhQCTy(tkyrI2+u-hlwuKX=O-bYZRy13HX)3;8hcf(+ z>}Jv?1aXs{Tjz2ft~Kpk1{xU5%;|3Eun+rB50!>_QE6cp^4saABgi$$IV)cjk^AOK z5R6m$wj<)+F*K*&V7MBO5VclA!MZOC!mB+l^XtB76z+^CePRN=?b1%-8i%79bDm#e zY5Zh7P`v-y{+XKWXq8_lDShL2_2!w*3Is({!3L5=YlH~=FJ{_(i#nI5_eR&9(@`2j z>JmNj81Z%ceAGCfBg~x9s9@WtuFfpM@c{QocHYOsj|D^ZglaXVa=WegZRXim3tq?& z;RYt568y*Lyv(PQpP zem+u#N^?tDFIX?nvwOK~9x^&>$^$EE9Dlt&@t%p(6F`47O2O&b8np3g0;1i%0Tz9H zqBCb3Jvr?pzaFi0<8c*!y0zW}k$(GH{HX(Au>Wug{<{uRk2?s;08v%gfZiO#c@oc2G=3=STAH}NYCms)>8aFGt3+@ zF<8-9dv394o=`FR?PZ%gyWVJ`5IBLEM8#LiQZN@tw;!_G&In9e>7&890;}!@<|3fk zU<7hIIr0(|Hm)XoXaLTHH_=9E*Y7M?W@%x!f9-sU4C|NKxKvj7HR;jJ30LLz@p49~ z3%OKBIDP&ly3L^>0rkvP`A7WlLLVMgBjITY^5?@won$HvT4{!j@3%jcGr7S|gp%rG z9djhZaWmbduUS&6Q9=jhzilu^aLdsj>|d+EVmMZ{R1ewGm?Ew{)X-Sx^nNCwQxtdu zC^nH%`bu@7e`NZ_xBSIMSZ9tZ$z5wjiy`cEY_4k1`V3{eI6eB8+1-gSDR$1KJD_YH zyC-(QrGgs?xIKNM62yWsmf-i%?pUZ+Ct#B>o)8`?_A^UFR=k}oBefH6GleLQlh99TbXNKgUAV8>Ny8bl%_PF?!JvXuqnCm$n|Kq8nSMKR*{? z^~SQz<;8iu5^y!OldZir7i~^UdRIz^4Z4OMLqWq?J&>G9U<_`tnUDuX-0KQoE`TbgdfOMmK=HU`YFs8-6@NU5HMD0=$Vv| z0S!3e_dAHo?9|bTO?DdfJe>7zy5O!~f0(4Bey!2cvF_K_T5JZGuCFVI)|8JcN+^ZE z71FP0i=WBV(|5Nf;l)GrKs80`G=b;cg%+tFVUjMIOt$XI&s^#{reJ=NCh5MwDwy00 z`DE(Bye^(X&8<7A$Ub#8l>|h=<5nn@H|JOtK>}3(LiHex<3HA>9$@H>()Ewz zW3RFjAYpLL{DGhXN!|b!gM(+`{h|d4c*Wa&k;6cmBrs)=qfW)lB(DUOQ2}2L%N=~` zhs%WAk_JloIAh~ctZ{u!0uftj46fGZk6=|$+FkYz=+RIyIr0yEL9*P)kA7^8DaZgW z@Ms7n?t_paD_CG5W7S%ewE#1g0r4DKaxj;Dnk6UcW#mm<=cG=HDx*!FIubn_N{XfY1W;3cGass$CJ~nP{p|XI9RAAcgkF_smF-c;69|^9f>^ z&k%K`Bdau`I3zd@kpZ!FNEk|R+HaOYD=VBSId-e3rzT5-S7FZ~)0#>UfMrZ!UjmMpKE&7` z=uSg6CLCXd(eX28AOrXaJ)jV0%Gluoz_uViU5;6NmrcTSERQ(~%ZNUd>&>ZaOF|dU zcUCDh(^9nlCS{?-CoJIxHA)f{1l7{!bA+i7(&r8yQHwwYN8G4C!#|^27<}q9Wk z767;eezvd%NWbz~O}a+U$$`-G=Z7Cc9X(5Err<3 z*+>s9k_ILKj~ud7nqf}tHP(T$EJnL(JA>Ni2{?34M}<%J3N;PRn#_H~z|bqRM0rJr z)*^*I!&}%&p$W^L0x;~|^SERZD80TXMtJ?o?8AF3S^pVPVz~vLVBSgIT z3KBs}Dt9Bi_o`DDTNoCVw{QJKDY4?Y#Q5Rrjq_!suyCC;HLd?)RkXjZ{prJWd&E5) z)4ah+SW6zKYftS@>_WQ=^cDr9RV&)C*k)LrQ;5$v7ol}XM+8~auo<>gcH*JgU1!f9 z(ex#Qtl=$|uN-oe<4f7|$CIlQ3R|;nckC3+m?-IcK_OSJ^f*5!_1YPm&IX!X;RY>L zOo<_BjBD5@vbrFnL)H0k#hu%R?)Y$u&2bv+(_z{DBL~YqvLjhG^T^?qeU-|){uV}2 z7wRS)tasUEX*&DzEhH%ohM;TmBMt6W`7aJLXky}c5liqzcPcmO<+aHG$O<)x05y>J ziMR*L;P`-zEG~iSsAZp!u|++0ovZe%!EL`GfzV}Lf@S6ov0}k;8>2Wk35!9zZ0!YDTRZ3 zZGji22Yd!1Vpwk9zS)Lxb3eKX#ic`xCy?Hgc>BK1qy^+D+Gwb~1kDr)B`t?XQ&Qdq z1<&gTE#+he&n#WGT+;FsEzH;&Ei_StElN&xT>U+cyaZtk9^n7QNu;SmWFM))VG0}x z&63#2Ws8KJqF?ah6_1V&eEYPGAdtq0JjbnR_-kQ|947Z5Jiae=V1Ej+k_{L_mv~?C zH_;9;NqlJ7#uzwA25!Plz3(mr%$CLn^C=Xtj2~6cXEaAlaL!$xc985KL=`R&{qIqk$6X}a+V~7@Hvcd z7cFL)s%8A(q_rjK9= zz49Ry>n6MN-44mR#APeQM{U9wq>KTE(*7L`NtpuZg=8ZEQ^V(+{>@cDk1&HhKd ziIkv2gqM&6k`V25VitFO-T-kyB(S2u-#fghR+Wj8Ft(UCK<8g1+@l|GM?z%)4<z0_dp69il=MIXUM$H%H zKML``=)DY)phpbvw0&2&-*ZQqCXDyts;a*gRXNC74;xO6dOfrcD-JKn&cxgdIEwr3 zazVg{RhXoR!RD(MiL_F)Il&LyVGW=&=a5VqkA{&2Q(aKYiF#1mWdU$34K$XdnU89otR>g12Sy@ok_*+WE*VYKu(J36YB$~$Pw6g(omd)PxDEQWZbZH-9$5V*jV{$ z&pN{K4lCxnRqq6P0uIr5@)*p3(Qwl|7~a2*04roj+%d6=eptWk(8(OPyh>dPYkcjq7|Fq%_tZC^7U97=BZ0>XAHu`()bxggzB5jX z)WvA%`;`M7c0c>?w#|r?6Pl1%eH=ElR}5~csgeXNeOJDF90P>JHMepSIDhRfRT7gF z6b?R8dOUh>vIsfb98l>BCNoV*N-SK`tE{SZ^J|<;1Brk%89ezWw)56Z(S*dWKAAui zo`!6#xj&t5WG&u2u*)3!x`m9<{X6(9qRvh&IC4KwGcj}evDiZdkw0adw&vMV8p)^y z0=?TnS>5aX2VFP*;F@4HD^B~?$iku^Tz2tq8_uGLtcBP?C}ev`ohNI2^NYqhW zRw~^4ba-im^F&FFcdvHraFfL73ZGryE$V&9VOCvaMy6@>kAONeb759T)H9JMHbPSl zFS0kezQPp-6QK^dbh1siq$gNxv7`H8nkzQG5#C2hHPor6D%vwS#45@W%$DIeAQDv( zM}Rj>ks(k;2b~G4B|?M2m5I)+suw~`J;?=eUE04wl5x{TCqbKlK>Q7grya=dhf**w z3dd-LL=3eb@-7}Ig;bXTFi-vEY^nlq7@}BR8t*pdN4d`bXp`p@YH8`Y8s0=eGHir; ztu$G`Q3+`-3}f1D{B9jwIn3BLF$xy~T6dx@x;k@0d=zE1+)5KoO{-@}Fi`3^B5P_# zM6c8gXr>MQ;pE)Hi~pos9=w~j2oAwTwe}_(PF&8t*{$c*$SGawqcC$p>493pw^bd+ zrp7kx@Cw!YcGj(03tX@wd=PL|{XstZiA2>^c~HBbY1)7_7g8f?kFYD$L6~I3ehuy= z@lO&c4{lDK3JPQNL_aGMsD}25IC12-8f0Bh=KFD?37!Dns=0 z0mOwk6L$<8&amRXyz8YNNkmh-n?e=@`{)anw@XQh9Nlx_ZH6R#r2rVRW=DmO3@yz9 zWRM$KM-_ZL%od~FndL3kz1hrye6y7!6t4eHvP zpB>}rp+kxJOAD@WwmjurI#)g=vhwGwABpACj;wB^QT)4Xd@7QcVzsK|$w_8fP^Cd{ zJy>qwv?P5t5YhaM?aPgI;8PHLcFO1H?xj?|CvU6>&FrWGEPZR2R=ebg_>}w1kcQH| zhNGy5-HUsMbAcVD+hPkNJCG^@*vVek7RTnJmDj+y>%{*&Xk;Z1@mGT=$4q5nMu`JNG5p8_j-4ARYP`Nzy~@ z<;*g|e1lIz<#K8B2)>>b^8wP{1Kek8yZ8L{%cdTmSBIOki>tf9`F)Fq=ES|tS7f`5 z29Nn+q!U~M*UaY451P7URV+A_bj~^oD-8*|q|LHpG+J5J-^knpyR@*-FJh#oofT z0=I`e!Z&GVBY4BLL!!ol2)jyoe;i1l?gf@p7PmTKi51#^8Wi zD0tTO&m2nCw*M63Hv@K0!-(53%eIUs-kqOgPP`D^M9NpE#)c>dY+?2_t$dt=!nA(& z$;GV$YQ3u}9}}l~UC%<0j7Kqh`302|sxshV&qEIu52yMy8A_oz*76e;d*rzYSm%ns z_H;zVYE9jCAFdD`qMApsAK$$CJj}z_t9t_oYhdCI#vNx zA)Lo8h)b>>2dsDe04|(7d?NkmkVXmJ0v=pRG@}1>O;fmNKvd&CMbfY$)WmHO%GS*L z0`w{b_VXB(jxI~*F z5=KgY;rD&@o7?6++NB~@kdtE*u0G{K>$fHA+e<)PZnoH&MO*uWVIkk=84tISVUMKT zZ1H7Yzn73iv)p2`BMVih7i}J9=`K4&y(#YlP9~bF=z01#BcUN|amSnvSSonZ#8_^6 z(tlo1I&z~u&%p9t+>Zf7p9sF%m|6R=OE(M*0DmH~hKDyBlR8$(xE)Pl`d9~=X8(p9 zB?VlnOh?a*UbuGH&enby=AVPhd8_D3UG`q3afUOdV8;&vG}R>u)Dfhe=?r52y7*1y zCfF8kFDm)Tp6S?rrW3PZor&BrpswGyOQn8@ctk#k$drQ9A^xhDIUNDp*pkH;^Lnav z^wt;m+km2*9|QX_%NS%1+XC#sT=uAYa^EYEz@Q`%oU~6a81F(@m~sW3s{N-iK?NV2 z$j2(ik4TYr3@mDz9yr~Fe5 zEa+MdTi-K=ZbG46EvzY=SO7D*RnA?Kzoy6mxduu>*lLBeK_m7~y;=Ih5s39W^vLUN zr1Gz4j9=kxEQ@M&ito$r~s0(Ni|r zY3FmR4Y0Qb>mx%G7P!uXHbsNM0kaldwY8pcL|BGg+dK!=W>NZvWJ+iPCl*b zrScXXpRq3SdKu|Q!9D{F6QPC3EJ|{8Ql-T6+=0^!tGFPd{S+%S>}4z4N%jUVS7n`9 zI8#q^4SdKw!QfC=zf$=ExKL}?LX&Gy%g4(m>MfY*YyvJlE!mE5S>C@!x1mVx+nG2#{{SFfv^_Tk+fm`r6s z6#U9sI`m~L<4g&KO*i&BLrH@HbHgV3%U6eY?F`Wm!^%?At><~{@9_+1@s~-a^I^ro zfih-w%d{8y``+HxVs%X^EiRYcdEMpeFZ#!Znc?Btsz9MGEGu(xFHLumKe=zpUrP_m-+2eGgr+J7hO>m~gjS6$hO6-{|HWgHcF^CG{i9j}p;pj>RJCii>+@X69(-VoX@(Y^!%3 zgRz)A)?^4u*th6m{Og*EfE~u)%F30V{F0ato37^sh1%u?+kGc{RA)qDixwux^vD*p z#JXowBgajIGFH&jf&E$zwIM7WInl+wWhmO|Go~) z*;s!j5@tRVb55$L&^U5)yWTziMld}MZEhZNLp#NoKJXIu8rAOcFbp0Umrd_>l%vpJ!;cGV9;r2#MtXLI3wJ<+Mq= za=Gq494FpvOAc@KwSM~oHU8B@ePq=$ko7~oH8ldG%xPRQqIHMOZKoO61hzEx0_nW% zQ-r8N-#&} z0^m(BXI6rXl|to52Oo0+KcTC4D`-tLHNu$nA|hQhuFKx1=a5IjcMV>rTm*7+X_h*) zPcJnlR&bm?(+NJxcR z@S;eFDKa6@=iFUfK3U)9SX=Ilj4i%5OBN3vh%cA3;LSzTz1T&>j4l#LS0k8#6;i0b zZy7RbW9CK)4<#hu8QHBpDgW^w5G5#QO0H^TwP~pBwIonqyK(#k6yKL3F*uhZ3La3) zdR+&-^3o+k3^#p~vgZEK#g?`d2Bju=qL>(L%N; z0i3;7%7|p(7Rs0*-iZKb+9*L2I|dLGW=*i+9&KY|6hSeDcF`Dym~dZ=g9U!{e6ujU zd>luLEN5h>v^Xqnx?A{(^hbm z!ma)-=#U0VIR5abJSTnyV_BG(23+$#cN_q;C>Gl~g&=mk%_TfTVDh(G7sORYp~6t;;tgm6rNcIr zuCy+b+adU1!6y$nqsKHRc;l-l`7HS+7@N5rDAXlor~Fpn#hm@uo?Is1LL3WG?KMov zx|1k7?gVWRXC$tJm3N~aT;0W&1+twgdp4JWg|hcOZBN>*?^r|iY4H*>3YW9?oyWoF z%(836)%qpFP1-)Xt_OCV!tj6sz5U}{x#X3?+&vI&_w5Zj1V{AeLgJpmcK5P>HLfO<(dGU)W5&R_jJid!Q{2uWw()B2sGHaP9Iq{i%y(|92{U&DnyqiIyCREJM zsM{XMJGvFSRkvq4r&C&w*8ZAWyKANl3orpB4y>p6LmyVR!qW+bh ziVKF9@{B}ojG324>BuPD9X#n9Rz_FIyqO4-C{q=PhF^L$0aiL;sY4(jisWk+MQ^93 z(la+n!lA+SA8hS!!Gec2u%7(UFLr?*3^tDgpr+~zluVE>13TqBO(??sf*~q|4dr$U zGsvRF^Z`vaaZM_8$i3MHR_gonDJF+>ClIO>Cyj=FTn6+2E{VkyCWW>}GO>&z#w0O8 zjOVzLUZ{zZa#rW+oyg1cUI4%;{Q3XE0Qm20|392FCOUfh{~L40K>vSif&XI8L~L!G zgp3^x9n9^VY#nI-)0NS;Hl`Ki|Ib1xWo}^XpzmaEYoloFU~WP!U~6RrLHo};Fm=RZ z`q%h(xPXAIn(QTU|Dp%_7J#SDN4Uddt^2b;NZ`yNXaTQA20H;c9*WY(0h)B;sX};mqLPjABwM z^H1w=I#(AbX~8wk2x@w(C%btYKh6Q2fDpYaAz0j1p7rmGZ`3J0!eb$@3VkW(oFd;8 zMmi;I&g=man^B^7Nw;0DIfQEiTw!+T$W(}cCd9Z9NB+zfWT2`a)JoT3x(Zoz1(SFN z_F5d_1B7-P#@x8u%BkfWUQ-^zT$*&tqXffT&EYNj+#^%OiWRs86gO5ZURWEdubdUm zT;;bOfVsc!g#VL7{@={I|2*yg8+fPW;B5R~i3sRB8vl3C|F?CQC4@?A99Xhvw2L%mAMFiQ*hcHHkknpFLpCuF81P`3VEnNDGzGal; z&?FqxR4~1sF;N^V(KbtpA;vl(PU>WBC@S=A7F^}B_qk*EfQa$*Yu9(icV@S2x3a_8 zveWs}MtS<}>;=Gp2r-Co;)B|okl2y(Z%`aA+~1aG&Jn;xX^4}31pFmtwg!Dk@n_Cn z!k|w;d=34<6Hn0RTsBsCgFcGpQC66z=U1M({0z8bw!bE}LVYt&-5OK6LtgtM)Q7KC z^toKo@0Mx4-j8=5Jv+$)-1v|)hCRNLyWE7k%U;Q(MdYZD{?*=^ASp0hu1JrMVDPC? zP_faki38>WD7*s`q9ULoKD*Dv@Q<_yZzJ#^Z?WD{X-jon$8P+E$j%;t^F}RVDw*5+ zPVJ6dI=VJGJ-l#Kagc(-!^Fk=V(RX~O$lGW*MwLEG6<+Ah>+;=xvk!9ibI1ti?};K zQncwmhrgMHaE!HwNA3|!!T>TPeNfJlS4@_0VDWmo|p*B-)JGo39xm;nh$wq|h{^rkw zQ@>BUVKyJVb}Fz5!$#frK^Q>Wt~ zUoQYN**FI%aVGr`r*@z%Rp(mE^$x=iLk-2QROY%}WIHPx3te^Q{tIZCytVo^-rHI0 z&5aa=$j8RS!@~Ij?RaOi_G-kD>qc{zLe(OB><~p&!Zy+%3c}+^xRxm|)=w{;0H*0Qr#OqPl*(_(M6Of=!$HqrZJAUGT zB?`n3W9DSx{uz11)Q+->1Q}|r7ErOpuBi z&EtJ=m>leAY}zdDd-ab9>Zws^pT|Evog9i=iaG~b|--4 z4DId2$okbSwQcjVpVKt>#6d*tc6+ZtrG&cgcP?VOCSW8NrBEidCt>NG6%HCh(tgMfaVxb4dJ+4wQgu+$f+nsFnbp9+YVak)1plK7J3kEA_$UVSFeOQNe~ zZ2(xy1wZ+G5@Z4mdexegc%^j|phV1uKSYlm2ZJyXRW1Mg$LTs3jiVH!7u(ShEGs-Y zosB+Dma`|yyA&n~wS%D>Z6CO3r19bqn&* z0VMI7*%b+4%?$Nf{}jOSlPNeg&4-7XRk7!%;zeeF-E|8E!4)P(qg8>U%7PTY?C(Hw z--Xp!PHVckbI6h;=C(e|qZxnZsQEa7;?%m%ybZ$$nyWJ_UGi4i+l#767$U{Kj6*@{ zTyv3|C*utM6v=RW`Z;m$EexR~I5@N6pe-iYTykr%q1qrI)%+;s%oAXaeo>P;d$jTe zzDYhul-OfiOC`OlCOV$Qj1c4c>|lFcJ88N{a@s;QtTC4;W+9QKHz{~j0dYUNi0Lvs zkx8tPM}HZ2J{>|^sG=kx(o9UkSczjQdZi50)r!NGE$SfwhIl-bStB%h#cKH7>LKao z@^{SjH`55zX(UjXFo(|xz$$z8O}zeiS_$}(0g3G8(i@+a+~XCVt)aHUZe zu6XNBz=wh#z=w$$cM2AXI8g7``W~wy>3e~cl+nHPD^IM4F^RlyBb=FDKK5lqgC-To zqbYj0=aC!7WukS(_hT~JK?hpKGxh-=8^2=5X&mjICRJ8sG94D`ss*Fer6^^AAY{9W zOh&3!$XEvD&TB~0YXA~iB!&ebKFAFxivGJK?7I!n_!5c4%|P9=r7~y+MMhv zZCdkD_aIo)z>*XEO15j-^6Hh6zB_Vl3&tvR%z8{ou!ii7YptbyiqRlT$si(gv)2ur z-?2G#WWj(+0#PT+*l~1gry!IAB_+fsL=tPkbs%8Ov57HYV8JZt$sNjCcsgexGIg_e zMKh`BmRDGGISi}dUbia%U(GX8e1AzeH;r0U*6!lQkGZ};!k<5@C)c%*KH_uhIdcX! zJ5~)h_g8pbIHi@Nz!d~$Kwtn^Ix9yFMDFH(+&7k<4G8XHR8#3L@M5xf^LXQT<30&b zx;l4zebmfSQ|C!8bk(-3jPf2X9qWOVjg?q;YLxesu6wM<~+#eJ|)q{LpWH!OAB5*%?+Z#q-BqM z2Y3dK9WAehv3ZMwUly^^2AIV=jkF0{Frx9$$)(~uF#vgu!FzB!u@9~0LTN+J!(~d# z6fxj{`x62s;^&3KMFo)vhzej)a$VAfRf-+EsAjoslEhgh$6AdO;fFTsSZl0MPkth>HQNf4VFGmSBd?_drq2kqU z9pRYGbQcyD+PSO-%$a#C22QFTkDM%TxD?;+v_MB`DNszcp0aLl^Egi_>fGdKQ+hHN z_~$k>Vxj5@6%fUA)klBAfG4gWNlMr$!oO|7vod!Y&{#mCQQdQCwnt-F>oxqn>&q5p zI61Gf-{YHFp0!zAl3wDs#U^w13s<*}F%UuO-NB-z(K-1eDlgAZ6rU++qDs4}sg)5l zqaKGB1%+Nxw<5ZJ(x0t}9W53`zxGnD&`vPm0_oWY1#PlcKr+HAKz)PK&SwbF&I006 zo$kni2H78*R2}|secrUKfPI_2xuuwz$m=;@u`?lfqNm;Fpk0<{)8C(aWuvB@>iG{C zjQpCN)qc5I`r z@y+?+siPo)BWD3B>5?>eNN_eJD*V9&8|p^)fVUC0brKT?*0ir0E+s@lsD+oHgyuvbO_R5AH18U4W#ghHl*K1l4#uf%!UGZu}o8-bLz)^N@OVeKVJ>GC6`9T!h zZVLwFlb_T1J;F;V^>Cbg+MOU2HS)kGuwPeX0*^)BrQpf}U3H{odSKZ8MP+M0!VT;o zwyjSXAwP~J?oKS@SS57=eGL90%ih##ko~ugfOJ3--^RRMb3LBg-XD_Ala}0fz?MWXyp7Zwt@=H97tGEm zxsyd+HPgHTE!Gh9`Ju>sdD^9}XHFgvltS5SBl4i5 z`weGKakT^+QY&pTqvy~=q69&dcnLv7pPgHmwcc|;yT*V3h1qcn7~fT zvbL}%1e`P2e^e0nyl>q0lA zvhX{i9pkKHui&!|w1vldZrguAM#Q44?E^c)FE9>7q4>i28Hk`rp~we|_#J@|RiU#Y z)O6DMnKFvlCUBdO5wI4;rG6?BT$vE~^AqRA$4v4w51ij9&1b_A=QbvYO86LEpx{QI zLYuOD!LFk@lRY#@H4eK#bng3$U@^QWy_9Ti4iRFC0y(fW2E?`e4uxV^Nt_~PuvX2X zQA(ZJb_HJc*icJVZ>DUxb1J3N$LUHo=x>EK04W$wZG_)iSQSfBXGM ze7{#iCguowja38_4V?hL4czZzeYfbFE%_658u@PP&KM7``s{xoZa?@GjsQ|`U)H^( z7a!!io2}X4rMw)_(DhZ|t__*|W3q1Jf(;y&Y4EwTjI($;_tX;x%~hFG$Fq^zm*Vn)}N6 z)%25r8}l}ACwE;e&g%<=xRiS={+xZNxEm4FMPAi(wf?*etSTcfBoQr8N9Z%TF<>k_*t)pbncW)Wlb&;V z)3CZ8n_dG0P!x)Z{%!CW?dujTE3zA>aCUgl3<@Wv-wkSII4=;t%|KW!@Rtpwc6293 zJI3DBz>Wc=(Xi4maBZ$WO%4rE73vL5E3#kKpf`Me)zFUW#Ukn^ygvr5Dx6=sid`hq zKu>Z0=R8r$)poS)bQ$J#SdR(hn_g@vj~pbJUaJC%!r+Gi#CCMAlRhk8w7gB19Z?rv z6SZ*K3M{N5Ca%JGkyJN~7nKvSu$85qV^hZ@XmtIJC)}RY(CW`tJ=&E%8RybVgh8WW z(ufs(1me52NzapVhy#VAG-#uY5@(Cb z@wITYVP=Ijg#)DIn?kno$$CqAp8)EewhdEhC@+i5wwXlwJOwIv%y8);1^Rvjz<8zB z=p}&iz>be0u1N0Ex+aY>Nu*8_&SAZOxJ_{aW#wb^z^;gN^(*;lraTMleEFD_kP5Kn zS+dV5Haph5RzJ^QseWr^0tzk%%l&IGUk!FmitcC_fX2l6n@iznsX1D{N;k|;lOLPe za+$!%Z&Pj1Rb3Zi4aZxxdHJ;lZrZu!Rm~x;D$m7A9829}Yjbla4cOvLm>P{_&ffeL z7qV`TQ<*x`t;h@yP5VCk%*1+;rOdZ=NShPo(m_YTIk^TT>oG<$Qz$cGo|37}Lf(qf z034VI*>EM=0sbFyreZg_ZdYjFrb4keAV7W!V9LPoy<)ss2Z=aKFoykejXJg;D>#vW zJfOt>@V#)nT&MsIBN*i&ZTo2E{6sHs8bd7srRHK$W%CG&vk>O#B)Etrg5^9XbLH2+ z)Du;F2Rj@gz}%BT4ra@uZr_;(rLp1O(Xdi-7o% znh-sp;YZrAltJBgm9m9sJ4DPfz}mF%E9EdNN(}WfBLcX;bL)CZ@jJ8P-W%shW~7kYnIS{G7x-SG`M@u_{H4Jq-i7Hzy;-D%EW-hE9M98<>5B#F6LG=wAe;=ShiVCj z(S>g9vdDuK$x%Qd$)M;^bt#p>Lkfl?9JopZgM0cj>cVkRKKZU=_fDgme)*?BEVga@ zjuYAf@0r;g%y8O1Phc80&Qsp*zN0DG=cY*#5SPrhome(@4*5jpgHzQE+ajG5c6y1= zLu!B1n#cwSz|(NxUA*0c3XVbNT1aKP?qx6 z`)ic@@JnVPFPdnEMyss<`O9Lr=C_u&qhDG7ycx5x)1uv?-DnOkBx;qnhR_0!X=?Zc zojkPM$|~zk+3w<+{589 zTc!Zrw1St>_F_RJDc12w4TtMR!rRj4^1WIrGw&X7*4;B-1>owTN^&G@VpjAHyg(NE z6^-DBRmd#k0JYMV`3Ejhe2Tualfv2xBWwE7c+lmbF4bK>^B({3GLw0mc? zTj}oVSx#QIJ1OItT|_d=>39N#odR~U^JuUEE*bOKIL%$92d|gblG=gRWM|;0sjA!% zZz9fBpWHV7JFE**UN@e(U}Y=0E>K%4LrMkz{LIa2O*Z0i>57Va|BacfV?){kqh=Gs%=`uti; z4`)|CaK_!q=L`GI$L8g17T|p)Hbn6^G*h+hK6s+}3M_r{1PnQuv@5Y(I%;oK)TUR(MX1z)kd%%DmB{Ec0|4z~*)1#Do2nb{Q_2xYkd`Eq+;XgXP<)shKg z%(ZKvreGi^UZIF*8k+&VJBqmwAvvO=0FChRh!bXmF?rdAbn+QWTs8fZfc98i7t0v- z&(kNMZ=Yq8B%ZrNkdW7#kDkjRhVzqkA-kIXPH)p1AA z8$In<$Aj4%}5kmieS%;uO?(Y5$l$gKxLI`jeWPdjbezwNZ{XQ!6* zxZ2zOe(DJqt0@ps`q%yyIbB=Ih2#B(mY=LzK!0Nz6WZZRvnad_BIsH z;Ptro?(FH#?S43omgWLYP6nc@i0Z;{T)0JIT+x|xw{3$4oS7}GGc zW~gSmNADEVE;!Oy$ER5QDUzC6Az-IM2_JxH9L|bf&(H&&y}uSo95iDXfC$txTSUXg zh=zAfTpyCJn`-e34t9EGqo^LX=AU=*`S?oT622-%tBM?nuA5-+iV01uXDrbup(h9CIQ4Nhu)avWp%eu^n%(3HdwA^YQbzT})?Z}%0#;)<1%5u6N z0)ED>nG72yr^!R4X*>Iinh02I{Ll6%;PL$mfE3X}tU;uySp^0_MGF&Ikc$pyL<|f< z5?d33l-n_bSA(0?t_BI6_u&_pEAD7yy=q5KMJBFTj%L8e(5}qc5K=m+Q&BbjVL&*z z{$_6ti_FwNiuzHOUT=BO`11I~;+bAswT0Un@`FUB5T--$qft2n2t&GwAc@sx7 zvN!H@($-I8c8JwFG;l{Uy9~e5RV}P6rA@gR7wQ(tP-t80=~x?lm|S9 z!aLx&gg7{%D25PQ7DBEb6)LbOx)56)uBcQNAnB*B31ZmyW0JIv%O zn)xUh7S4{XuB=&qJI=qbUY2Uim7?S{_w5g8hECRu7zkYUyx9lgK6bQnmodEhsl7(? zw05oR*LwnWbUx2AV+z&ea4X`pN-n7?xWN{|ZAuQJXWmhlVxe@c0*4MhyV%oUP4u!mO-)cRgG!<(PI6M^Kmu)M`-&l>z|B`}(e&W< zTG_riZy%7c-w_npIvgqPOzZ08^}f*0$fqksq$5>2na`Q_kw1+mm@df9rbi^;vJ%=ErEs| z(Vir$>kNy^KL%k<8-#Nrudu2&DrBNh86;Ms4y_o4S;CmOOLdbIQ?Et)<{5$a?OW$3 zL`;No_@t&6o=_ncqCnvUNIN>8(O=R-DP>|oWn22i!j&hr_%@oY;~Aw<=`bt24Oa_q zGgEa(xy8sxe*fz^ck{FT`SCRRclzYnbOUwF7%_Y}Qv>p(cDLDe_2d27PcqI>1ZHw!8V&~bwhQe~DllEvv(<^^k0$=1nD zTFS?;_N`fdyJ_?r-Gl&fF^=8SXij#1q5Vw|z(GixzCHnb3JS?p`}*Si2N|cbdm{w3 zTC2spPPxGdy2K0^8gs>f1BeAtA-l@N2nJ6zHURS_5rG$Ox7 z5*z>WQ@4E|U1>mNwjz_`(q7J?2o{X=F&?M$?3JNhtd&jBR9kFn1^oV=eNO5T$wg^K_)ffUh!XmNNCxZG)Uu%oAqVOF{Qh zw}P{JM^wfXg=SzgzhyJ?Sjt(>}^nm2~a^Tu4N0KmklJKM0#T5_I zQ&jS3Tqz-dlQ3?he@?gjN(8jy=IKIvk9V^iC)U|B^ygVk-~J!!N8rCb3^J!!JNz&C zP_gl80|p0Fc0S}Ipiapg!lUOWA0rNql|%^f1WX9v(aPq<^!h@){sL049^HHG_8h6* zwMg9qY2ev~uO4WI!pbzA0VzJRCmbM#iB%Z~Ht@7e5Y3JTbd4Vsw?oEWq!VvM#ao^p zn;*5Um>Cn+j;JliuUA(0Dckk=`*-`+ncj|`C?~VEl|r!+HrfmHm%2OTVX(CB&q!A1 zJY78`qcA(R7uYD;!xQx<#h4h6Z1%x2jQ4i|Jj8L&O->2?pIed9nj9C!^~;Uq=y~Fi;xMk5rhI@fK6s6IlBOe_vIdgN7~c`3cZn z{xJDvnN0z;-<6lAp6%DGF3y6T|M|j*SmX^n=-#@Q6@_(FR^%STl-xs;ULcd0;8`jY zDE>1&&{ikPMxKQjRQMO{Vz_8|`yL#?+85x0G`RAwauB?}0J8(>EDM=E@;Dyqs4F<% znB|>x!Nq=*)tR!j3yN!Fp?Gr;$KD#EY*`C)-Uxz;M(p_@CQnL(|>tD;X&j2!UDG{NYv~o7AFta?@YPmJ?(5j2&*7l02IIzNep<# zh{jDi6r2ho$qhs%F-gS}B$G7B=71J14NtCyz6sx425qlSV{mb||rbwk*1#gNy1f^Wr4YL*1)A5AA_xm%D zp9dhBqYQ|P$T5(Q^CaB)LXJ(@8XTgKENc14&Dt)RgEosBw+i3Lg(Kowb7gG!dqIdZ zk}?*Cq$AH}pld?g+TcoW0cS|$qCoMDPa9>)i|L-eR-xN2dE%d8*)*+#0-Qlt%`O(J z7|UulD~Rn0(I+n^2JOo0pQ>U7lZZss1;H?EKDMVr*jOx7_x?|~AfU^Q@8_`R zqZV18z{IOG7t;0xiB4n*ZgA{i5lBLQb~33U)bH9Nl3It&Zr?k`JK8&gI{mlN9=i`R zp6-w&FrK;3JqO;qmFcTz*e^PFK1av2dgRX(l&Yh$Zu+UHe|65D|K0~2*%M3qXeI$K zOS$+z5l}C0SB3Bw-&XwS8j%cPj-$O@=n_d{gr=K95PDV@qU3N1zaIiSN2bs~)1X28 zt8}h87;Aj?c)sh3d#0;KAbw&Ay1ip-`t_Y%QIjP*G1fg%)qN(FIXgEAV64`4!%*teFY!Kq)?=Kq0@h~R@^9=oWnDKxB^;x>+ zQ|lRD$?qEQa<10686PGjOhpOO)9m-%HWS6r^4%fv62I^Vk~X6b zi-J?B+RPmmdy*3P8!i2_r-38}>5L)pXXp7LOr~xEn<^a|%*Wcl&nTB0Qf_#TQP|&C$rPn1 zI^4|9n~J4oGvz~5DrL_mD;~d3wR<`bh9{8}I1W)~{%6GB%G{~m;U1HU0og{d7b5C| zx$QuJuN#2JIJWA2Hn2(V+{XZ)+Bpq*+c;E@bx@GL>z`WZf)UhGn=!MCuuP>#X+ZtPz&X+2m!B5xjP zoERX2Xg+A*wh)VT4kwoPbq@oQ_dX!c!G!uZ>9BB-Jvcs`Qr`P6D}1bi)!?k5D@<8ng`oW~qJaAob`)IbJ!mUL1LJpK zCT~;d=I+K@2q5v=?9X0(%*a5 zOKTMhZ;6ni>)8IBncA|GTgyI4+B6QPH0bT?#)RHR>nSA+#7NJ;l^7@30T;aOfls58 zuD;~zC8)plk~gO~ha}6Ln@LUwJ-?t@g#-+lSqdAtG(e$+bMyOWu2Gyp8CySF_gXbA zFZUV~ju+e419LfWv?s_Y=uo4LO_OM_gTimYT0d9l?A%%e~0@E?Aq)^3n4M&g`GZtVy zD+=PRS#3)b(j%?n=pju0=4DlgF8=}bkdX2~qTlbwD)uegeiQm#slBn%a=%-4zLp)y zFw$)5*w}8d7EX%o-I}lWwO01XUSQws)-QU$AO1Oswlg?%By?2ml`7HoKxU%NPk+9R zy&x`ZdG8w*6i%K$LS1;?SMKDH@#)WWJnAM*F@8}AgZtqmlia$?onZO|7aAKn7p@g^ zMu9D&&N0r>nWdT4p9P-%%rTF3V|Hx*rG{la0EiroJ>Jq(dx{U0bl+2lm2&I54L2Z; zaoYX&$^q_#t~zf5`pa=+Q>w&f|I+n9k}l)c5z+isE8Sn)YzX(!|B!A(xkwb(cMD{&D{5=TbYvI7RXg%&**^f7~CpcthM`{ah=JoT}0m# z)+1YNinq%2UG~BetK}~Nx0pKIv_&eoOY^{U6o=F3U5Pw4AoUcKHkIV{jDm{WjLMw;5oHcbcC(>8~mYxmjB z{cnGXcJ(62ZojE$m~f%>akdS$XD~yf$>3$yv&=4jOTH;17U}SmD5FKJ`}pj2rdT%J zu@Ybdl3C+L3VHRj2KNABI>G1T*i4t~(OvMIs9D5g|#lPdnG5=1PFq=JI(tL#BN}q`$ z4*u)GQcc+Y6}sN!zXWc;Vol@<&Ul?U<~gvq-jvRj*KWIiH`ZM!h{+NIDzTCZQb9p^ z3Zw${1bX0oXzVYnxvC0;Vzq-M2S4(+5YP~?kQ3yC@~clgc9 z=eh3;ZE~ujEK5<|vGJ*K?01mQ?g=wwWCT8IZ#6yYI{mmjE`_b|`R#5YJBZU&2`5 z+TBX@;US@8;e0lytICsc{3@Z%@W(#&{BMob*G|0ukAHnWw5I(DGUjNjl}-TQ21f|s zZp}+rVSBPg^`NG`J&UQ`#9u=|K!BC6e>r(3FOR+|nN&oBZJ@Wrg+^gkc$njC;Bg2IIY|ed5lRQW>}eYOAY!~dhefdT z6LigCR>D;fQuV*y28=znEp#O(Aa~q;x7ZfiA?-kx%<(8z)&S>yPYex6`GOQaxIUxs zov`+xV<=v+3DRJgB!R!yzmj!79L=~Y3_)+2^-jeQ=mQ;9Cm;>jqGK9be%{rhV=B$5 z@8an~b1MCRJ7{AJ-$Ykc z)p9$PTM9Zh=Z^+F+|m8s+@J^a*GKwo>Y5IxZt3W;zkAdMT%5V|)t(l4%=6B(-`v~rul`>?ZTzsBHC^TBv4XKCW^0`` z|K-bz^#krla}BYl((WvZc@fm-d30S}=!ReA?)uaG@B=Q}TpC>Fh5IkN((CHDIeXnZ z*JEQ!`}MJ=C#6HBx-(g|Gy7-v`PUYv>^h0HoDJZPJCeT|^5&mq*Nt_aXP+wMlO?C8S`Rt@) zZcb6O`;CjP-5ICO-<VPdF|T__{L|-~qjHDygn8fU^PVW|R`S2{v=8`Wk9}pX zp*vB#!KYB~+VLW+)0pU3DNpO{iX2g_KRC#9Dfuo}6#bX$BP$fjn^ldWkC!=zMu!C< zWnUK@&Wbt|`KaPX#if5NE_i>^r_V(&DYF4)G zf~xKu!O3|Ek0WfnoQMnaaT}>LZuKmu!@hn_Uv3xvKHnq4;cS7T-ZuSMTV~b;u~f=Y zjH)Ez#jaB^XXH4vg%K2mzhU{PH!RoBa)~@ZCBDri0)A`?B#})`Ac-i6aA@p*A`(Hn zsewcs;%AmX8vD&8#(_i@!I^?ao6MiZ7min_&r*whef z_~^vSnlnXLy+%6~ge>*)Qm*oZ{tp@b)yw0DTdKjI66f6#Yo6WJ#|;m)4!7FP=^s#} zZ}0A2U%)l})9%E=iW;iOy?#k-;e%++;Hgzh9c!v@w(Po}|D4%n!cylgIT<3aZ@zu# z&|vZ0#oXvQ!o=&w^+m(QMpaXJzIsWOqhDq*m0>S**2V=b59<=AKC@ZVDBkGO9Q}0j zqq9C0Jx3zqf3e}th<)vy+VAzdeV*oG%r1x2cQ?%IJC}ZMwy|R~H?wLW_VC$(l=e$) zw~Kck)J@m<=(etRHRaK#59+@4)s>_>>-Jt)FzQSXEBp+fu8lH;N4XCx+)Ve#idMZ~ zd#k>#$ksHydH0i8X`88g`|c->`*^yos`=nYEYhHxc1X9a&uJi~nq90MJbxenzCh`0L@4&SO(; zZ2HaOzuv(=15^ITjXWhs$)YVlN{XV)ASHw;AwUUXN?4yLaW)%)H$ijEB^%V?b8s^t zdBCd>Gmbf%dILvZME0DIyH4nbB6RRW%M2I*&T{263d5qu0EW<~_8 z{9GnY24e3-41mh!maxu3zzf;~brJ!uu{sNxkdnxR4EPH^1yRflRI8-HvgBxHpg_b7 zk*!Sz%_Z?N{#fBXR`6mbW6RbAb|BDa21?YBjmE~NB$jDZTD4TBLCo_IGyP<7atY+D z;j}0WN7x)ZhUuwRX_LTAf|(&|iBgjUZAep@{9u4ruaQe-Vox5^8%YKnFPF>Y122Fs zGzg@o;n*iIYOpM?U1i?5|L*}|&?6QjvJNFUA`OFS?H0xxzW@ub9Rqilk33EjjhGDs z7#-S&Qoc$Hx5A{!6V|K2mjGy86%Q5!bUD5r3}!%*Oz9zo7<}}YO=d)76r$9wUOo1P z0^`0CW&9^HrE6&L!pX}`Orq5JYMBI3_HH7fNQ9y!8^G>3ih@)3Pyi*P6o?WwKDMdi zv{KoG9*aMwLjhg0KG0)<_7pXKgZ2~wZXgFLC;>|$C^2Bm+$9*d0G#&siC_tiVr&!# z? zVjPfFLBLzICD{b!XAO*lyzHhq69cM;I&?|S^^G(P{XVqEKiexZORm@RLO`rjKQ~uw{n#VF-IKy zAV8uKH-U&pf&fB!LYfv}0v?}(iwGgdgCck$7Uto>W&C#)m^%b2DL+9bRcN${2-5@O x34}t7O?e0@51fyqI4vYNJd(vBXz;y=!UUj6mZ+1>;RIHfWT6aaXJLSd@oxl44SN6p literal 0 HcmV?d00001 From 720b5f4d25caa2b8ada836046080d93469c45e3f Mon Sep 17 00:00:00 2001 From: Ugo Date: Wed, 26 Apr 2017 10:15:21 -0700 Subject: [PATCH 117/183] new tuning --- arduino/arduino_nano328_node/arduino_nano328_node.ino | 2 +- workspace/src/barc/launch/run_lmpc_remote.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 3 ++- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 8 ++++---- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index 4939ba0d..6b4d85d3 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -83,7 +83,7 @@ class Car { // Motor limits // TODO are these the real limits? - const float MOTOR_MAX = 120.0; + const float MOTOR_MAX = 150.0; const float MOTOR_MIN = 40.0; const float MOTOR_NEUTRAL = 90.0; // Optional: smaller values for testing safety diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index b0b042a8..7e53c1ae 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -8,7 +8,7 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 1b793fb6..f4be641e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -247,7 +247,8 @@ function main() solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) acc_f[1] = mpcSol.z[1,5] acc0 = mpcSol.z[2,5] - else # otherwise: use system-ID-model + else + # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) zCurr[i,7] = acc0 solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 80a29208..138e06e6 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -50,13 +50,13 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f - mpcParams.QderivZ = 1.0*[1,1,1,1,1,0] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,100.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 2.0 # scaling of Q-function + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[5.0,10.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams_pF.N = 15 + mpcParams_pF.N = 16 mpcParams_pF.Q = [0.0,50.0,0.1,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states From 859e3f26d9db89cf05070d7f19e89550fc7cc057 Mon Sep 17 00:00:00 2001 From: francescoricciuti Date: Tue, 26 Sep 2017 16:41:55 -0700 Subject: [PATCH 118/183] changes 26 sept 17 --- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 138e06e6..d1536e48 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -45,7 +45,7 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) - mpcParams.N = 12 + mpcParams.N = 7 mpcParams.Q = [5.0,0.0,0.0,1.0,10.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) From a4d907a04003a3041bef9e357f58dc9ff1ffc7ff Mon Sep 17 00:00:00 2001 From: francescoricciuti Date: Wed, 8 Nov 2017 14:50:18 -0800 Subject: [PATCH 119/183] changed frequency of pos_info, refined tuning, refined system ID --- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 51 ++++++++-------- .../src/barc/src/barc_lib/LMPC/functions.jl | 2 +- workspace/src/barc/src/barc_simulator_dyn.jl | 59 +++++++++++++++++-- .../state_estimation_SensorKinematicModel.py | 2 +- 4 files changed, 81 insertions(+), 33 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index 54e396a7..b803338e 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -146,18 +146,19 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo n_sys_ID_post = 60 # steps of sysID after current point in previous laps n_sys_ID_prev_c = 30 # steps of sysID before current point in current lap + freq_ratio = 4 # ratio between the frequency at which the informations about the position are updated in the topic and the frequency at which the controller calculates and updates the control input # vec_range_ID = () # for i=1:n_laps_sysID # vec_range_ID = tuple(vec_range_ID...,idx_s[i]-n_prev:idx_s[i]+n_ahead) # related index range # end - sysID_idx_diff = idx_s[1]-n_sys_ID_prev*5-1 + (1:5:(n_sys_ID_prev+n_sys_ID_post+1)*5) # these are the indices that are used for differences + sysID_idx_diff = idx_s[1]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences sysID_idx = sysID_idx_diff[1:end-1] - sysID_idx_diff2 = idx_s[2]-n_sys_ID_prev*5-1 + (1:5:(n_sys_ID_prev+n_sys_ID_post+1)*5) # these are the indices that are used for differences + sysID_idx_diff2 = idx_s[2]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences sysID_idx2 = sysID_idx_diff2[1:end-1] - sysID_idx_diff_c = cC - (n_sys_ID_prev_c+1)*5-1 + (1:5:(n_sys_ID_prev_c+1)*5) + sysID_idx_diff_c = cC - (n_sys_ID_prev_c+1)*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev_c+1)*freq_ratio) sysID_idx_c = sysID_idx_diff_c[1:end-1] sz1 = size(sysID_idx,1) @@ -166,39 +167,39 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo sz = sz1 + sz2 # psiDot - y_psi = zeros((2*sz1+sz2)*5) - A_psi = zeros((2*sz1+sz2)*5,3) - for i=0:4 + y_psi = zeros((2*sz1+sz2)*freq_ratio) + A_psi = zeros((2*sz1+sz2)*freq_ratio,3) + for i=0:freq_ratio-1 y_psi[(1:sz1)+i*sz1] = diff(oldpsiDot[sysID_idx_diff+i]) A_psi[(1:sz1)+i*sz1,:] = [oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i]] - #y_psi[(1:sz2)+i*sz2+5*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) - #A_psi[(1:sz2)+i*sz2+5*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*5,2,cL]] - y_psi[(1:sz1)+i*sz1+5*sz1+5*sz2] = diff(oldpsiDot[sysID_idx_diff2+i]) - A_psi[(1:sz1)+i*sz1+5*sz1+5*sz2,:] = [oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i]] + #y_psi[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) + #A_psi[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2,cL]] + y_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2] = diff(oldpsiDot[sysID_idx_diff2+i]) + A_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2,:] = [oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i]] end # xDot - y_xDot = zeros((2*sz1+sz2)*5) - A_xDot = zeros((2*sz1+sz2)*5,3) - for i=0:4 + y_xDot = zeros((2*sz1+sz2)*freq_ratio) + A_xDot = zeros((2*sz1+sz2)*freq_ratio,3) + for i=0:freq_ratio-1 y_xDot[(1:sz1)+i*sz1] = diff(oldxDot[sysID_idx_diff+i]) A_xDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i].*oldpsiDot[sysID_idx+i] oldxDot[sysID_idx+i] oldacc[sysID_idx+i]] - #y_xDot[(1:sz2)+i*sz2+5*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) - #A_xDot[(1:sz2)+i*sz2+5*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] - y_xDot[(1:sz1)+i*sz1+5*(sz1+sz2)] = diff(oldxDot[sysID_idx_diff2+i]) - A_xDot[(1:sz1)+i*sz1+5*(sz1+sz2),:] = [oldyDot[sysID_idx2+i].*oldpsiDot[sysID_idx2+i] oldxDot[sysID_idx2+i] oldacc[sysID_idx2+i]] + #y_xDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) + #A_xDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] + y_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldxDot[sysID_idx_diff2+i]) + A_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot[sysID_idx2+i].*oldpsiDot[sysID_idx2+i] oldxDot[sysID_idx2+i] oldacc[sysID_idx2+i]] end # yDot - y_yDot = zeros((2*sz1+sz2)*5) - A_yDot = zeros((2*sz1+sz2)*5,4) - for i=0:4 + y_yDot = zeros((2*sz1+sz2)*freq_ratio) + A_yDot = zeros((2*sz1+sz2)*freq_ratio,4) + for i=0:freq_ratio-1 y_yDot[(1:sz1)+i*sz1] = diff(oldyDot[sysID_idx_diff+i]) - A_yDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i].*oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i-delay_df*5]] - #y_yDot[(1:sz2)+i*sz2+5*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) - #A_yDot[(1:sz2)+i*sz2+5*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*5,2]] - y_yDot[(1:sz1)+i*sz1+5*(sz1+sz2)] = diff(oldyDot[sysID_idx_diff2+i]) - A_yDot[(1:sz1)+i*sz1+5*(sz1+sz2),:] = [oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i].*oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i-delay_df*5]] + A_yDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i].*oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i-delay_df*freq_ratio]] + #y_yDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) + #A_yDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2]] + y_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldyDot[sysID_idx_diff2+i]) + A_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i].*oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i-delay_df*5]] end # if any(isnan,y_yDot) # check if any value in the y_yDot value is NaN diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index d1536e48..dedd5302 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -51,7 +51,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,10.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.QderivU = 1.0*[5.0,100.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 495e7406..e469c51c 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -137,8 +137,22 @@ function main() # IMU measurements if i%2 == 0 # 50 Hz imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) - yaw = z_current[i,5] + 0.002*randn()* + imu_drift - psiDot = z_current[i,6] + 0.001*randn() + rand_yaw = 0.01*randn() + if rand_yaw > 0.1 + rand_yaw = 0.1 + elseif rand_yaw<-0.1 + rand_yaw=-0.1 + end + + yaw = z_current[i,5] + imu_drift + rand_yaw#+ 0.002*randn() + + rand_psiDot = 0.01*randn() + if rand_psiDot > 0.1 + rand_psiDot = 0.1 + elseif rand_psiDot<-0.1 + rand_psiDot=-0.1 + end + psiDot = z_current[i,6] +rand_psiDot#+ 0.001*randn() imu_meas.t_msg[imu_meas.i] = t imu_meas.t[imu_meas.i] = t imu_meas.z[imu_meas.i,:] = [yaw psiDot] @@ -146,8 +160,24 @@ function main() imu_data.orientation = geometry_msgs.msg.Quaternion(cos(yaw/2), sin(yaw/2), 0, 0) imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros - imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] + randn()*0.3*0.0 - imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] + randn()*0.3*0.0 + + rand_accX = 0.001*randn() + if rand_accX > 0.1 + rand_accX = 0.1 + elseif rand_accX<-0.1 + rand_accX=-0.1 + end + + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] #+rand_accX#+ randn()*0.3*1.0 + + rand_accY = 0.001*randn() + if rand_accY > 0.1 + rand_accY = 0.1 + elseif rand_accY<-0.1 + rand_accY=-0.1 + end + + imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] #+rand_accY#+ randn()*0.3*1.0 publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) end @@ -180,8 +210,25 @@ function main() # GPS measurements if i%6 == 0 # 16 Hz - x = round(z_current[i,1] + 0.002*randn(),2) # Indoor gps measures, rounded on cm - y = round(z_current[i,2] + 0.002*randn(),2) + + rand_x = 0.01*randn() + if rand_x > 0.1 + rand_x = 0.1 + elseif rand_x<-0.1 + rand_x=-0.1 + end + + x = round(z_current[i,1] + rand_x,2)#0.002*randn(),2) # Indoor gps measures, rounded on cm + + rand_y = 0.01*randn() + if rand_y > 0.1 + rand_y = 0.1 + elseif rand_y<-0.1 + rand_y=-0.1 + end + + y = round(z_current[i,2] + rand_y,2)#0.002*randn(),2) + if randn()>10 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) x += 1#randn() # add random value to x and y y -= 1#randn() diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 57957331..2642a471 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -178,7 +178,7 @@ def state_estimation(): vhMdl = (L_f, L_r) # set node rate - loop_rate = 50 + loop_rate = 40 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) se.t0 = rospy.get_rostime().to_sec() # set initial time From ed51a0e3ac783f4c42205419f2632c2bca24caf5 Mon Sep 17 00:00:00 2001 From: francescoricciuti Date: Wed, 8 Nov 2017 16:00:59 -0800 Subject: [PATCH 120/183] first steps towards obstacle avoidance --- workspace/src/barc/src/LMPC_node.jl | 90 +++++++++++++++++-- .../src/barc/src/barc_lib/LMPC/functions.jl | 31 ++++++- workspace/src/barc/src/barc_lib/classes.jl | 38 ++++++++ 3 files changed, 152 insertions(+), 7 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index f4be641e..5731b1d3 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -87,9 +87,13 @@ function main() modelParams = ModelParams() mpcParams = MpcParams() mpcParams_pF = MpcParams() # for 1st lap (path following) + obstacle = Obstacle() + selectedStates = SelectedStates() + oldSS = SafeSetData() - InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize) - mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) + InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize,obstacle,selectedStates,oldSS) + + mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) # CHANGE THIS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) max_N = max(mpcParams.N,mpcParams_pF.N) @@ -102,8 +106,8 @@ function main() coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables - log_coeff_Cost = NaN*ones(mpcCoeff.order+1,2,10000) - log_coeff_Const = NaN*ones(mpcCoeff.order+1,2,5,10000) + log_coeff_Cost = NaN*ones(mpcCoeff.order+1,2,10000) # DONT NEED THIS + log_coeff_Const = NaN*ones(mpcCoeff.order+1,2,5,10000) # DONT NEED THIS log_sol_z = NaN*ones(max_N+1,7,10000) log_sol_u = NaN*ones(max_N,2,10000) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) @@ -120,6 +124,11 @@ function main() log_step_diff = zeros(10000,5) log_t_solv = zeros(10000) log_sol_status = Array(Symbol,10000) + + selStates_log = zeros(selectedStates.Nl*selectedStates.Np,6,buffersize,30) #array to log the selected states in every iteration of every lap + statesCost_log = zeros(selectedStates.Nl*selectedStates.Np,buffersize,30) #array to log the selected states' costs in every iteration of every lap + log_predicted_sol = zeros(mpcParams.N+1,7,buffersize,30) + log_onestep = zeros(buffersize,6,30) acc_f = [0.0] @@ -139,6 +148,9 @@ function main() uCurr = zeros(10000,2) # contains input information step_diff = zeros(5) + obs_curr = zeros(buffersize,3,obstacle.n_obs)::Array{Float64,3} # info about the obstacle in the current lap + + # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 @@ -175,6 +187,15 @@ function main() acc0 = 0.0 opt_count = 0 + same_sPF = 0 + same_sLMPC = 0 + + # #### Set initial conditions on the obstacles + + # obs_curr[1,1,:] = obstacle.s_obs_init + # obs_curr[1,2,:] = obstacle.ey_obs_init + # obs_curr[1,3,:] = obstacle.v_obs_init + # Start node while ! is_shutdown() if z_est[6] > 0 # check if data has been received (s > 0) @@ -188,8 +209,20 @@ function main() publish(pub, cmd) # ============================= Initialize iteration parameters ============================= i = lapStatus.currentIt # current iteration number, just to make notation shorter + + # Check if and how many times the states are repeated + if i>1 + if z_est[6] == zCurr[i-1,6] && lapStatus.currentLap <= n_pf + same_sPF +=1 + elseif z_est[6] == zCurr[i-1,6] && lapStatus.currentLap > n_pf + same_sLMPC +=1 + end + end + zCurr[i,:] = copy(z_est) # update state information posInfo.s = zCurr[i,6] # update position info + lap_now = lapStatus.currentLap + #trackCoeff.coeffCurvature = copy(coeffCurvature_update) # ============================= Pre-Logging (before solving) ================================ @@ -201,10 +234,34 @@ function main() end log_step_diff[k+1,:] = step_diff + if lapStatus.currentLap > n_pf + if lapStatus.currentIt>1 + log_onestep[lapStatus.currentIt,:,lapStatus.currentLap] = abs(mpcSol.z[1:6] - zCurr[1:6]) + end + end + # ======================================= Lap trigger ======================================= if lapStatus.nextLap # if we are switching to the next lap... println("Finishing one lap at iteration ",i) # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj + + oldSS.oldCost[lapStatus.currentLap-1] = lapStatus.currentIt + cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target + #save the terminal cost + for j = 1:buffersize + cost2target[j] = (lapStatus.currentIt-j+1) # why do i need Q_cost? + end + oldSS.cost2target[:,lapStatus.currentLap-1] = cost2target + + + # if lapStatus.currentLap == obstacle.lap_active # if its time to put the obstacles in the track + # obstacle.obstacle_active = true # tell the system to put the obstacles on the track + # end + # if lapStatus.currentLap > obstacle.lap_active # initialize current obstacle states with final states from the previous lap + # obs_curr[1,:,:] = obs_curr[i,:,:] + # end + + zCurr[1,:] = zCurr[i,:] # copy current state i = 1 lapStatus.currentIt = 1 # reset current iteration @@ -224,6 +281,9 @@ function main() end end + oldSS.oldSS[lapStatus.currentIt,:,lapStatus.currentLap] = z_est + + # ======================================= Calculate input ======================================= #println("*** NEW ITERATION # ",i," ***") println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt) @@ -231,6 +291,17 @@ function main() #println("s = $(posInfo.s)") #println("s_total = $(posInfo.s%posInfo.s_target)") + #mpcParams.Q_obs = ones(selectedStates.Nl*selectedStates.Np) + + + + if lapStatus.currentLap > 1 + if lapStatus.currentIt == (oldSS.postbuff+2) + oldSS.oldSS[oldSS.oldCost[lapStatus.currentLap-1]:oldSS.oldCost[lapStatus.currentLap-1]+oldSS.postbuff+1,1:5,lapStatus.currentLap-1] = zCurr[1:oldSS.postbuff+2,1:5] + oldSS.oldSS[oldSS.oldCost[lapStatus.currentLap-1]:oldSS.oldCost[lapStatus.currentLap-1]+oldSS.postbuff+1,6,lapStatus.currentLap-1] = zCurr[1:oldSS.postbuff+2,6] + posInfo.s_target + end + end + # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() @@ -288,8 +359,8 @@ function main() log_sol_status[k] = mpcSol.solverStatus log_state[k,:] = zCurr[i,:] log_cmd[k+1,:] = uCurr[i,:] # the command is going to be pubished in the next iteration - log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost - log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst + log_coeff_Cost[:,:,k] = mpcCoeff.coeffCost # DONT NEED THIS + log_coeff_Const[:,:,:,k] = mpcCoeff.coeffConst # DONT NEED THIS log_cost[k,:] = mpcSol.cost log_curv[k,:] = trackCoeff.coeffCurvature log_state_x[k,:] = x_est @@ -304,6 +375,10 @@ function main() log_sol_u[1:mpcParams.N,:,k] = mpcSol.u end + if lapStatus.currentLap > n_pf + log_predicted_sol[:,:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.z + end + # Count one up: lapStatus.currentIt += 1 else @@ -325,6 +400,9 @@ function main() "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k]) println("Exiting LMPC node. Saved data to $log_path.") + println("number of same s in path following = ",same_sPF) + println("number of same s in learning MPC = ",same_sLMPC) + end if ! isinteractive() diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index dedd5302..37359047 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -44,7 +44,15 @@ function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{F end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, - posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64) + posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, + obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) + + selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + Nl = selectedStates.Nl + selectedStates.selStates = zeros(Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(Nl*selectedStates.Np) + mpcParams.N = 7 mpcParams.Q = [5.0,0.0,0.0,1.0,10.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following @@ -88,6 +96,16 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track oldTraj.idx_start = zeros(30) oldTraj.idx_end = zeros(30) + oldSS.oldSS = NaN*ones(buffersize,7,30) # contains data from previous laps usefull to build the safe set + oldSS.oldSS_xy = NaN*ones(buffersize,4,30) + oldSS.cost2target = zeros(buffersize,30) # cost to arrive at the target, i.e. how many iterations from the start to the end of the lap + oldSS.oldCost = ones(Int64,30) # contains costs of laps + oldSS.count = ones(30)*2 # contains the counter for each lap + oldSS.prebuff = 30 + oldSS.postbuff = 40 + oldSS.idx_start = ones(30) # index of the first measurement with s > 0 + oldSS.idx_end = zeros(30) # index of the last measurement with s < s_target + mpcCoeff.order = 5 mpcCoeff.coeffCost = zeros(mpcCoeff.order+1,2) mpcCoeff.coeffConst = zeros(mpcCoeff.order+1,2,5) @@ -98,6 +116,17 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentLap = 1 # initialize lap number lapStatus.currentIt = 0 # current iteration in lap + + obstacle.obstacle_active = false # true if we have t consider the obstacles in the optimization problem + obstacle.lap_active = 20 # number of the first lap in which the obstacles are used + obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) + obstacle.n_obs = 1 # number of obstacles + obstacle.s_obs_init = [20] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [-0.8] # initial ey coordinate of each obstacle + obstacle.v_obs_init = [1.5] # initial velocity of each obstacles + obstacle.r_s = 0.5 + obstacle.r_ey = 0.2 + obstacle.inv_step = 3 # number of step of invariance required for the safe set end # Use this function to smooth data (moving average) diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index dfad9d97..3c1611f7 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -39,6 +39,29 @@ type OldTrajectory # information about previous trajectories OldTrajectory(oldTraj=Float64[],oldInput=Float64[],oldTimes=Float64[],oldCost=Float64[],count=Int64[],prebuf=50,postbuf=50,idx_start=Int64[],idx_end=Int64[]) = new(oldTraj,oldInput,oldTimes,oldCost,count,prebuf,postbuf,idx_start,idx_end) end +type SafeSetData + oldSS::Array{Float64} # contains data from previous laps usefull to build the safe set + cost2target::Array{Float64} # cost to arrive at the target, i.e. how many iterations from the start to the end of the lap + oldCost::Array{Int64} # contains costs of laps + count::Array{Int64} # contains the counter for each lap + prebuff::Int64 + postbuff::Int64 + idx_start::Array{Int64} # index of the first measurement with s > 0 + idx_end::Array{Int64} # index of the last measurement with s < s_target + oldSS_xy::Array{Float64} + + SafeSetData(oldSS=Float64[],cost2target=Float64[],oldCost=Int64[],count=Int64[],prebuf=50,postbuf=50,idx_start=Int64[],idx_end=Int64[],oldSS_xy=Float64[]) = + new(oldSS,cost2target,oldCost,count,prebuf,postbuf,idx_start,idx_end,oldSS_xy) +end + +type SelectedStates # Values needed for the convex hull formulation + selStates::Array{Float64} # selected states from previous laps ... + statesCost::Array{Float64} # ... and their related costs + Np::Int64 # number of states to select from each previous lap + Nl::Int64 # number of previous laps to include in the convex hull + SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2) = new(selStates,statesCost,Np,Nl) +end + type MpcParams # parameters for MPC solver N::Int64 nz::Int64 @@ -93,3 +116,18 @@ type ModelParams c_f::Float64 ModelParams(l_A=0.25,l_B=0.25,m=1.98,I_z=0.24,dt=0.1,u_lb=Float64[],u_ub=Float64[],z_lb=Float64[],z_ub=Float64[],c0=Float64[],c_f=0.0) = new(l_A,l_B,m,I_z,dt,u_lb,u_ub,z_lb,z_ub,c0,c_f) end + +type Obstacle + obstacle_active::Bool # true if we have to consider the obstacles in the optimization problem + lap_active::Int64 # number of the first lap in which the obstacles are used + obs_detect::Float64 # maximum distance at which we can detect obstacles (in terms of s!!) + n_obs::Int64 # number of obstacles in the track + s_obs_init::Array{Float64} # initial s coordinate of each obstacle + ey_obs_init::Array{Float64} # initial ey coordinate of each obstacle + v_obs_init::Array{Float64} # initial velocity of each obstacles + r_s::Float64 # radius on the s coordinate of the ellipse describing the obstacles + r_ey::Float64 # radius on the ey coordinate of the ellipse describing the obstacle + inv_step::Int64 # number of step of invariance required for the safe set + + Obstacle(obstacle_active=false,lap_active=10,obs_detect=1.0,n_obs=1,s_obs_init=Float64[],ey_obs_init=Float64[],v_obs_init=Float64[],r_s=0.5,r_ey=0.3,inv_step=1) = new(obstacle_active,lap_active,obs_detect,n_obs,s_obs_init,ey_obs_init,v_obs_init,r_s,r_ey,inv_step) +end \ No newline at end of file From 5bc8bf56e676fbe26d3e3fa3015150e2f7bc7b93 Mon Sep 17 00:00:00 2001 From: francescoricciuti Date: Mon, 13 Nov 2017 17:58:53 -0800 Subject: [PATCH 121/183] new tuning --- eval_sim/eval_data.jl | 257 ++++++++++++++++++ workspace/src/barc/src/LMPC_node.jl | 91 +++++-- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 246 ++++++++++++++++- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 58 +++- .../src/barc/src/barc_lib/LMPC/functions.jl | 12 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 61 ++++- workspace/src/barc/src/barc_lib/classes.jl | 9 +- workspace/src/barc/src/barc_simulator_dyn.jl | 6 +- 8 files changed, 703 insertions(+), 37 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index d372c36c..c83b7667 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -95,6 +95,263 @@ function eval_sim(code::AbstractString) grid() end +function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) + + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + + Data = load(log_path_LMPC) + + oldSS_xy = Data["oldSS_xy"] + oldSS = Data["oldSS"] + selectedStates = Data["selectedStates"] + selStates = Data["selStates"] + statesCost = Data["statesCost"] + pred_sol = Data["pred_sol"] + one_step_error = Data["one_step_error"] + lapStatus = Data["lapStatus"] + posInfo = Data["posInfo"] + eps_alpha = Data["eps_alpha"] + cvx = Data["cvx"] + cvy = Data["cvy"] + cpsi = Data["cpsi"] + + Nl = selectedStates.Nl + Np = selectedStates.Np + + + track = create_track(0.4) + + + + for i = laps + + vx_alpha = eps_alpha[1,1:200,i] + vy_alpha = eps_alpha[2,1:200,i] + psiDot_alpha = eps_alpha[3,1:200,i] + ePsi_alpha = eps_alpha[4,1:200,i] + eY_alpha = eps_alpha[5,1:200,i] + s_alpha = eps_alpha[6,1:200,i] + + cvx1 = cvx[1:200,1,i] + cvx2 = cvx[1:200,2,i] + cvx3 = cvx[1:200,3,i] + cvy1 = cvy[1:200,1,i] + cvy2 = cvy[1:200,2,i] + cvy3 = cvy[1:200,3,i] + cvy4 = cvy[1:200,4,i] + cpsi1 = cpsi[1:200,1,i] + cpsi2 = cpsi[1:200,2,i] + cpsi3 = cpsi[1:200,3,i] + + figure(1) + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") + plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + grid("on") + + + + t = linspace(1,200,200) + + figure(2) + + subplot(221) + plot(t,one_step_error[1:200,1,i]) + title("One step prediction error for v_x in lap $i") + ylim(-0.0001,findmax(one_step_error[1:200,1,i])[1]) + grid("on") + + subplot(222) + plot(t,one_step_error[1:200,2,i]) + ylim(-0.0001,findmax(one_step_error[1:200,2,i])[1]) + title("One step prediction error for v_y in lap $i") + grid("on") + + subplot(223) + plot(t,one_step_error[1:200,3,i]) + ylim(-0.0001,findmax(one_step_error[1:200,3,i])[1]) + title("One step prediction error for psiDot in lap $i") + grid("on") + + subplot(224) + plot(t,one_step_error[1:200,4,i]) + ylim(-0.0001,findmax(one_step_error[1:200,4,i])[1]) + title("One step prediction error for ePsi in lap $i") + grid("on") + + + figure(3) + + subplot(221) + plot(t,one_step_error[1:200,5,i]) + ylim(-0.0001,findmax(one_step_error[1:200,5,i])[1]) + title("One step prediction error for eY in lap $i") + grid("on") + + subplot(222) + plot(t,one_step_error[1:200,6,i]) + ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("One step prediction error for s in lap $i") + grid("on") + + figure(4) + + subplot(221) + plot(t,vx_alpha') + #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("Slack variable for vx in lap $i") + grid("on") + + subplot(222) + plot(t,vy_alpha') + #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("Slack variable for vy in lap $i") + grid("on") + + subplot(223) + plot(t,psiDot_alpha') + #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("Slack variable for psiDot in lap $i") + grid("on") + + subplot(224) + plot(t,ePsi_alpha') + #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("Slack variable for ePsi in lap $i") + grid("on") + + figure(5) + subplot(221) + plot(t,eY_alpha') + #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("Slack variable for eY in lap $i") + grid("on") + + subplot(222) + plot(t,s_alpha') + #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + title("Slack variable for s in lap $i") + grid("on") + + figure(6) + + subplot(221) + plot(t,cvx1,t,cvx2,t,cvx3) + legend(["cvx1","cvx2","cvx3"]) + title("C_Vx in lap $i") + grid("on") + + subplot(222) + plot(t,cvy1,t,cvy2,t,cvy3,t,cvy4) + legend(["cvx1","cvx2","cvx3","cvy4"]) + title("C_Vy in lap $i") + grid("on") + + subplot(223) + plot(t,cpsi1,t,cpsi2,t,cpsi3) + legend(["cpsi1","cpsi2","cpsi3"]) + title("C_Psi in lap $i") + grid("on") + + if switch == true + + + + for j = 2:2000 + + + + vx_pred = pred_sol[:,1,j,i] + vy_pred = pred_sol[:,2,j,i] + psiDot_pred = pred_sol[:,3,j,i] + ePsi_pred = pred_sol[:,4,j,i] + eY_pred = pred_sol[:,5,j,i] + s_pred = pred_sol[:,6,j,i] + + + oldvx = selStates[1:Np,1,j,i] + oldvx2 = selStates[Np+1:2*Np,1,j,i] + oldvx3 = selStates[2*Np+1:3*Np,1,j,i] + oldvy = selStates[1:Np,2,j,i] + oldvy2 = selStates[Np+1:2*Np,2,j,i] + oldvy3 = selStates[2*Np+1:3*Np,2,j,i] + oldpsiDot = selStates[1:Np,3,j,i] + oldpsiDot2 = selStates[Np+1:2*Np,3,j,i] + oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] + oldePsi = selStates[1:Np,4,j,i] + oldePsi2 = selStates[Np+1:2*Np,4,j,i] + oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] + oldeY = selStates[1:Np,5,j,i] + oldeY2 = selStates[Np+1:2*Np,5,j,i] + oldeY3 = selStates[2*Np+1:3*Np,5,j,i] + olds = selStates[1:Np,6,j,i] + olds2 = selStates[Np+1:2*Np,6,j,i] + olds3 = selStates[2*Np+1:3*Np,6,j,i] + + + + + + figure(7) + clf() + subplot(221) + plot(s_pred,vx_pred,"or") + plot(olds,oldvx,"b") + plot(olds2,oldvx2,"b") + plot(olds3,oldvx3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) + title("State vx in lap $i, iteration $j") + grid("on") + + subplot(222) + plot(s_pred,vy_pred,"or") + plot(olds,oldvy,"b") + plot(olds2,oldvy2,"b") + plot(olds3,oldvy3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) + title("State vy in lap $i, iteration $j ") + grid("on") + + subplot(223) + plot(s_pred,psiDot_pred,"or") + plot(olds,oldpsiDot,"b") + plot(olds2,oldpsiDot2,"b") + plot(olds3,oldpsiDot3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) + title("State psiDot in lap $i , iteration $j") + grid("on") + + subplot(224) + plot(s_pred,ePsi_pred,"or") + plot(olds,oldePsi,"b") + plot(olds2,oldePsi2,"b") + plot(olds3,oldePsi3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) + title("State ePsi in lap $i, iteration $j ") + grid("on") + + + figure(8) + clf() + subplot(221) + plot(s_pred,eY_pred,"or") + plot(olds,oldeY,"b") + plot(olds2,oldeY2,"b") + plot(olds3,oldeY3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) + title("State eY in lap $i, iteration $j ") + grid("on") + + + + + sleep(3) + end + end + end + +end + # THIS FUNCTION EVALUATES DATA THAT WAS RECORDED BY BARC_RECORD.JL # **************************************************************** function eval_run(code::AbstractString) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 5731b1d3..3a94982a 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -93,8 +93,10 @@ function main() InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize,obstacle,selectedStates,oldSS) - mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) # CHANGE THIS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) + mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) + mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + max_N = max(mpcParams.N,mpcParams_pF.N) # ROS-specific variables @@ -129,7 +131,11 @@ function main() statesCost_log = zeros(selectedStates.Nl*selectedStates.Np,buffersize,30) #array to log the selected states' costs in every iteration of every lap log_predicted_sol = zeros(mpcParams.N+1,7,buffersize,30) log_onestep = zeros(buffersize,6,30) - + log_epsalpha = zeros(6,buffersize,30) + log_cvx = zeros(buffersize,3,30) + log_cvy = zeros(buffersize,4,30) + log_cpsi = zeros(buffersize,3,30) + acc_f = [0.0] # Initialize ROS node and topics @@ -170,16 +176,30 @@ function main() # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) oldTraj.oldTraj[1:buffersize,6,2] = linspace(0,posInfo.s_target,buffersize) + oldTraj.oldTraj[1:buffersize,6,3] = linspace(0,posInfo.s_target,buffersize) + + + oldSS.oldSS[1:buffersize,6,1] = linspace(0.1,posInfo.s_target,buffersize) + oldSS.oldSS[1:buffersize,6,2] = linspace(0.1,posInfo.s_target,buffersize) + oldSS.oldSS[1:buffersize,6,3] = linspace(0.1,posInfo.s_target,buffersize) + posInfo.s = posInfo.s_target/2 - lapStatus.currentLap = 3 - oldTraj.count[3] = 500 - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) - oldTraj.count[3] = 1 + lapStatus.currentLap = 4 + oldTraj.count[4] = 500 + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus,selectedStates,oldSS) + oldTraj.count[4] = 1 lapStatus.currentLap = 1 oldTraj.oldTraj[1:buffersize,6,1] = NaN*ones(buffersize,1) oldTraj.oldTraj[1:buffersize,6,2] = NaN*ones(buffersize,1) + oldTraj.oldTraj[1:buffersize,6,3] = NaN*ones(buffersize,1) + posInfo.s = 0 + selectedStates.selStates = zeros(selectedStates.Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(selectedStates.Nl*selectedStates.Np) + oldSS.oldSS = NaN*ones(buffersize,7,30) + + uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) n_pf = 3 # number of first path-following laps (needs to be at least 2) @@ -271,22 +291,33 @@ function main() if lapStatus.currentLap <= n_pf setvalue(mdl_pF.z_Ol[:,1],mpcSol.z[:,1]-posInfo.s_target) elseif lapStatus.currentLap == n_pf+1 - setvalue(mdl.z_Ol[:,1],mpcSol.z[1:mpcParams.N+1,4]) - setvalue(mdl.z_Ol[:,6],mpcSol.z[1:mpcParams.N+1,1]-posInfo.s_target) - setvalue(mdl.z_Ol[:,5],mpcSol.z[1:mpcParams.N+1,2]) - setvalue(mdl.z_Ol[:,4],mpcSol.z[1:mpcParams.N+1,3]) + setvalue(mdl.z_Ol[1:mpcParams.N,1],mpcSol.z[1:mpcParams.N,4]) + setvalue(mdl.z_Ol[1:mpcParams.N,6],mpcSol.z[1:mpcParams.N,1]-posInfo.s_target) + setvalue(mdl.z_Ol[1:mpcParams.N,5],mpcSol.z[1:mpcParams.N,2]) + setvalue(mdl.z_Ol[1:mpcParams.N,4],mpcSol.z[1:mpcParams.N,3]) setvalue(mdl.u_Ol,mpcSol.u[1:mpcParams.N,:]) + + setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,1],mpcSol.z[1:mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,6],mpcSol.z[1:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,5],mpcSol.z[1:mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,4],mpcSol.z[1:mpcParams.N+1,3]) + setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) + setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) elseif lapStatus.currentLap > n_pf+1 setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + #setvalue(mdl_convhull.alpha[1:selectedStates.Nl*selectedStates.Np],1/(selectedStates.Nl*selectedStates.Np)) + end end oldSS.oldSS[lapStatus.currentIt,:,lapStatus.currentLap] = z_est + oldSS.oldSS_xy[lapStatus.currentIt,:,lapStatus.currentLap] = x_est # ======================================= Calculate input ======================================= #println("*** NEW ITERATION # ",i," ***") - println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt) + println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, ", s: ",posInfo.s) #println("State Nr. ", i, " = ", z_est) #println("s = $(posInfo.s)") #println("s_total = $(posInfo.s%posInfo.s_target)") @@ -302,12 +333,13 @@ function main() end end + # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus,selectedStates,oldSS) tt = toq() - println("Finished coefficients, t = ",tt," s") + #println("Finished coefficients, t = ",tt," s") end #println("Starting solving.") @@ -315,17 +347,28 @@ function main() tic() if lapStatus.currentLap <= n_pf z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2]),acc0] # use kinematic model and its states - solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev) + solveMpcProblem_pathFollow(mdl_pF,mpcSol,mpcParams_pF,trackCoeff,posInfo,modelParams,z_pf,uPrev,lapStatus) acc_f[1] = mpcSol.z[1,5] acc0 = mpcSol.z[2,5] else # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) zCurr[i,7] = acc0 - solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) + #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) + solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + acc0 = mpcSol.z[2,7] acc_f[1] = mpcSol.z[1,7] end + if lapStatus.currentLap>n_pf + # println("current s= ",posInfo.s) + # println("current lap= ",lapStatus.currentLap) + # println("selected states= ",selectedStates.selStates) + # println("states cost= ",selectedStates.statesCost) + # println("old safe set= ",oldSS.oldSS[lapStatus.currentIt:lapStatus.currentIt+10,6,lapStatus.currentLap-1]) + # println("current control= ", [mpcSol.a_x,mpcSol.d_f]) + end + log_t_solv[k+1] = toq() # Send command immediately, only if it is optimal! @@ -355,6 +398,13 @@ function main() # Logging # --------------------------- + + selStates_log[:,:,lapStatus.currentIt,lapStatus.currentLap] = selectedStates.selStates # array to log the selected states in every iteration of every lap + statesCost_log[:,lapStatus.currentIt,lapStatus.currentLap] = selectedStates.statesCost # array to log the selected states' costs in every iteration of every lap + log_cvx[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Vx + log_cvy[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Vy + log_cpsi[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Psi + k = k + 1 # counter log_sol_status[k] = mpcSol.solverStatus log_state[k,:] = zCurr[i,:] @@ -377,6 +427,7 @@ function main() if lapStatus.currentLap > n_pf log_predicted_sol[:,:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.z + #log_epsalpha[:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.eps_alpha end # Count one up: @@ -397,11 +448,17 @@ function main() "cost",log_cost[1:k,:],"curv",log_curv[1:k,:],"coeffCost",log_coeff_Cost,"coeffConst",log_coeff_Const, "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:], - "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k]) + "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k],"selectedStates",selectedStates,"one_step_error",log_onestep, + "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"lapStatus",lapStatus, + "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy) println("Exiting LMPC node. Saved data to $log_path.") println("number of same s in path following = ",same_sPF) println("number of same s in learning MPC = ",same_sLMPC) + # println("one step prediction error= ",log_onestep[1:20,:,lapStatus.currentLap]) + # println("postions= ",oldSS.oldSS[1:20,6,lapStatus.currentLap]) + # println("selected States= ",selStates_log[:,6,1:20,lapStatus.currentLap] ) + # println("states cost= ",statesCost_log[:,1:20,lapStatus.currentLap]) end diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 6736baf3..42b3d049 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -220,6 +220,7 @@ type MpcModel_pF z0::Array{JuMP.NonlinearParameter,1} coeff::Array{JuMP.NonlinearParameter,1} + z_Ref::Array{JuMP.NonlinearParameter,2} z_Ol::Array{JuMP.Variable,2} u_Ol::Array{JuMP.Variable,2} @@ -257,8 +258,8 @@ type MpcModel_pF n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation # Create function-specific parameters - z_Ref::Array{Float64,2} - z_Ref = cat(2,zeros(N+1,3),v_ref*ones(N+1,1)) # Reference trajectory: path following -> stay on line and keep constant velocity + z_ref::Array{Float64,2} + z_ref = cat(2,zeros(N+1,3),v_ref*ones(N+1,1)) # Reference trajectory: path following -> stay on line and keep constant velocity u_Ref = zeros(N,2) # Create Model @@ -287,6 +288,7 @@ type MpcModel_pF # end # end + @NLparameter(mdl, z_Ref[1:N+1,1:4] == 0) @NLparameter(mdl, z0[i=1:5] == 0) @NLparameter(mdl, uPrev[1:10,1:2] == 0) @@ -352,6 +354,7 @@ type MpcModel_pF m.mdl = mdl m.z0 = z0 + m.z_Ref = z_Ref m.coeff = coeff m.z_Ol = z_Ol m.u_Ol = u_Ol @@ -363,6 +366,245 @@ type MpcModel_pF end end +type MpcModel_convhull + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,1} + c_Vy::Array{JuMP.NonlinearParameter,1} + c_Psi::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + + + eps_lane::Array{JuMP.Variable,1} + #eps_alpha::Array{JuMP.Variable,1} + #eps_vel::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearExpression,1} + + derivCost::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + #slackCost::JuMP.NonlinearExpression + #velocityCost::JuMP.NonlinearExpression + + function MpcModel_convhull(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates) + + + m = new() + + #### Initialize parameters + + dt = modelParams.dt # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + u_lb = modelParams.u_lb # lower bounds for the control inputs + u_ub = modelParams.u_ub # upper bounds for the control inputs + z_lb = modelParams.z_lb # lower bounds for the states + z_ub = modelParams.z_ub # upper bounds for the states + c0 = modelParams.c0 + + + ey_max = trackCoeff.width/2 # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons + n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree for curvature approximation + v_max = 3 # maximum allowed velocity + + v_ref = mpcParams.vPathFollowing # reference velocity for the path following + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + #Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity + delay_df = mpcParams.delay_df + delay_a = mpcParams.delay_a + + + Np = selectedStates.Np::Int64 # how many states to select + Nl = selectedStates.Nl::Int64 # how many previous laps to select + + acc_f = 1.0 + + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.07))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @variable( mdl, z_Ol[1:(N+1),1:7]) + @variable( mdl, u_Ol[1:N,1:2]) + @variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + #@variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha + #@variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity + + + + z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states + z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_6s[j,i]) + setupperbound(u_Ol[j,i], u_ub_6s[j,i]) + end + end + for i=1:7 + for j=1:N+1 + setlowerbound(z_Ol[j,i], z_lb_6s[j,i]) + setupperbound(z_Ol[j,i], z_ub_6s[j,i]) + end + end + + @NLparameter(mdl, z0[i=1:7] == 0) + @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0) + @NLparameter(mdl, c_Vx[i=1:3] == 0) + @NLparameter(mdl, c_Vy[i=1:4] == 0) + @NLparameter(mdl, c_Psi[i=1:3] == 0) + @NLparameter(mdl, uPrev[1:10,1:2] == 0) + @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" + + + # Conditions for first solve: + setvalue(z0[1],1) + setvalue(c_Vx[3],0.1) + + @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i]) + + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) + #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity + @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull + + for n = 1:6 + @NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np}) # terminal constraint + #@NLconstraint(mdl,z_Ol[N+1,i] >= sum{alpha[j]*selStates[j,i],j=1:Nl*Np}-eps_alpha[i]) + #@NLconstraint(mdl,z_Ol[N+1,i] <= sum{alpha[j]*selStates[j,i],j=1:Nl*Np}+eps_alpha[i]) + end + + @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) + @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i])) + + println("Initializing model...") + + # System dynamics + for i=1:N + if i<=delay_df + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2]) # psiDot + else + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2]) # psiDot + end + if i<=delay_a + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1])) + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f) + else + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f) + end + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7]) # xDot + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1])) # xDot + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7]) + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i])) # ePsi + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY + @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s + end + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end + + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + end + + + + # Cost functions + + # Derivative cost + # --------------------------------- + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} + + QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ + QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) + + # Control Input cost + # --------------------------------- + @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+ + R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df}) + + # Lane cost (soft) + # --------------------------------- + @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) + + # Terminal Cost + # --------------------------------- + @NLexpression(mdl, terminalCost , sum{alpha[i]*statesCost[i], i=1:Nl*Np}) + + # Slack cost (soft) + # --------------------------------- + #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) + + # Velocity Cost + #---------------------------------- + #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) + + # Overall Cost function (objective of the minimization) + # ----------------------------------------------------- + + @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost ) #+ slackCost)#+ velocityCost) + + + + sol_stat=solve(mdl) + println("Finished solve 1: $sol_stat") + sol_stat=solve(mdl) + println("Finished solve 2: $sol_stat") + + + m.mdl = mdl + m.z0 = z0 + m.coeff = coeff + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.uPrev = uPrev + #m.eps_alpha=eps_alpha + + m.derivCost = derivCost + m.controlCost = controlCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + #m.velocityCost= velocityCost #velocity cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + + + return m + end +end + # why does the solution change when I start the simulation twice, totally independently? Is not everything initialized equally? # why do I get a failed restoration phase even though there are *almost* no constraints and the solution should be super clear? diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index b803338e..f6d4af20 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -16,7 +16,8 @@ # z[5] = eY # z[6] = s -function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams,lapStatus::LapStatus) +function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams,lapStatus::LapStatus, + selectedStates::SelectedStates,oldSS::SafeSetData) # this computes the coefficients for the cost and constraints # Outputs: @@ -27,6 +28,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo s = posInfo.s s_target = posInfo.s_target + Np = selectedStates.Np + Nl = selectedStates.Nl + # Parameters Order = mpcCoeff.order # interpolation order for cost and constraints pLength = mpcCoeff.pLength # interpolation length for polynomials @@ -38,11 +42,15 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo n_laps_sysID = 2 # number of previous laps that are used for sysID - selected_laps = zeros(Int64,2) - selected_laps[1] = lapStatus.currentLap-1 # use previous lap - selected_laps[2] = lapStatus.currentLap-2 # and the one before - if lapStatus.currentLap >= 5 - selected_laps[2] = indmin(oldTraj.oldCost[2:lapStatus.currentLap-2])+1 # and the best from all previous laps + selected_laps = zeros(Int64,Nl) + # selected_laps[1] = lapStatus.currentLap-1 # use previous lap + # selected_laps[2] = lapStatus.currentLap-2 # and the one before + # if lapStatus.currentLap >= 5 + # selected_laps[2] = indmin(oldTraj.oldCost[2:lapStatus.currentLap-2])+1 # and the best from all previous laps + # end + + for i = 1:Nl + selected_laps[i] = lapStatus.currentLap-i # use previous lap end # Select the old data @@ -57,7 +65,12 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo olddF = oldTraj.oldInput[:,2,selected_laps]::Array{Float64,3} #olddF = smooth(olddF,5) + oldS_safeSet = oldSS.oldSS[:,6,selected_laps]::Array{Float64,3} + #println("safe set= ",oldS_safeSet[1:20,:,:]) + N_points = size(oldTraj.oldTraj,1) # second dimension = length (=buffersize) + N_points2 = size(oldSS.oldSS,1) + s_total::Float64 # initialize DistS::Array{Float64} # initialize @@ -74,8 +87,41 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo # Compute the index DistS = ( s_total - oldS ).^2 + DistS2 = ( s_total - oldS_safeSet ).^2 + idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! + idx_s2= findmin(DistS2,1)[2] + + off = 1 + idx_s2 = idx_s2 + off + + + ####################################################################### + + for j = 0:(Nl-1) + + selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,m=1:6] = oldSS.oldSS[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]+Np-(j*N_points2)-1,i=1:6,selected_laps[j+1]] # select the states from lap j... + + selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = 0.7 * oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]] # and their cost + + # if obstacle.lap_active == true # if the obstacles are on the track, check if any of the selected states interferes with the propagated obstacle + + # for n=1:obstacle.n_obs + # ellipse_check = (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,6]-obs_prop_s[n])/r_s)^2) + (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,5]-obs_prop_ey[n])/r_ey)^2) + + # if any(x->x<=1, ellipse_check) == true # if any of the selected states is in the ellipse + + # index = find(ellipse_check.<=1) # find all the states in the ellipse + + # mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] = 10 # and set the values of the weight to 10, so that they are excluded from optimization + # end + # end + # end + + end + + ########################################################################## vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 37359047..68225899 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,13 +47,13 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.Np = 25 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 3 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) - mpcParams.N = 7 + mpcParams.N = 15 mpcParams.Q = [5.0,0.0,0.0,1.0,10.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) @@ -63,6 +63,10 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + + mpcParams_pF.N = 16 mpcParams_pF.Q = [0.0,50.0,0.1,10.0] @@ -102,7 +106,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track oldSS.oldCost = ones(Int64,30) # contains costs of laps oldSS.count = ones(30)*2 # contains the counter for each lap oldSS.prebuff = 30 - oldSS.postbuff = 40 + oldSS.postbuff = 50 oldSS.idx_start = ones(30) # index of the first measurement with s > 0 oldSS.idx_end = zeros(30) # index of the last measurement with s < s_target diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index eabdab28..bf5b9d09 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -55,7 +55,7 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara # println("c_Vx = $(mpcCoeff.c_Vx)") # println("c_Vy = $(mpcCoeff.c_Vy)") # println("c_Psi = $(getvalue(mdl.c_Psi))") - # println("ParInt = $(getvalue(mdl.ParInt))") + println("ParInt = $(getvalue(mdl.ParInt))") # #println("u_prev = $(getvalue(mdl.uPrev))") println("Solved, status = $sol_status") # println("Predict. to s = $(sol_z[end,6])") @@ -72,10 +72,17 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara nothing end -function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::MpcParams,trackCoeff::TrackCoeff,posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64}) +function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::MpcParams,trackCoeff::TrackCoeff,posInfo::PosInfo, + modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},lapStatus::LapStatus) # Load Parameters coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} + v_ref = mpcParams.vPathFollowing + + + z_ref1 = cat(2,zeros(mpcParams.N+1,3),v_ref*ones(mpcParams.N+1,1)) + z_ref2 = cat(2,zeros(mpcParams.N+1,1),0.2*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) + z_ref3 = cat(2,zeros(mpcParams.N+1,1),-0.2*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) sol_status::Symbol sol_u::Array{Float64,2} @@ -85,6 +92,13 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M setvalue(mdl.z0,zCurr) setvalue(mdl.uPrev,uPrev) setvalue(mdl.coeff,coeffCurvature) + if lapStatus.currentLap == 1 + setvalue(mdl.z_Ref,z_ref1) + elseif lapStatus.currentLap == 2 + setvalue(mdl.z_Ref,z_ref2) + elseif lapStatus.currentLap == 3 + setvalue(mdl.z_Ref,z_ref3) + end # Solve Problem and return solution sol_status = solve(mdl.mdl) @@ -109,4 +123,45 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M nothing end -# Cost structure: costZ, costZTerm, constZTerm, derivCost, controlCost, laneCost +function solveMpcProblem_convhull(m::MpcModel_convhull,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus, + posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},selectedStates::SelectedStates) + + # Load Parameters + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + selStates = selectedStates.selStates::Array{Float64,2} + statesCost = selectedStates.statesCost::Array{Float64,1} + + # Update current initial condition, curvature and System ID coefficients + setvalue(m.z0,zCurr) + setvalue(m.uPrev,uPrev) + setvalue(m.c_Vx,mpcCoeff.c_Vx) # System ID coefficients + setvalue(m.c_Vy,mpcCoeff.c_Vy) + setvalue(m.c_Psi,mpcCoeff.c_Psi) + setvalue(m.coeff,trackCoeff.coeffCurvature) # Track curvature + setvalue(m.selStates,selStates) + setvalue(m.statesCost,statesCost) + + # Solve Problem and return solution + sol_status = solve(m.mdl) + sol_u = getvalue(m.u_Ol) + sol_z = getvalue(m.z_Ol) + + # export data + mpcSol.a_x = sol_u[1,1] + mpcSol.d_f = sol_u[1,2] + mpcSol.u = sol_u + mpcSol.z = sol_z + #mpcSol.eps_alpha = getvalue(m.eps_alpha) + mpcSol.solverStatus = sol_status + mpcSol.cost = zeros(6) + mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] + #println("o,terminal,control,deriv,0,lane= ",mpcSol.cost) + + println("Solved, status = $sol_status") + + nothing +end + diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index 3c1611f7..e31cea21 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -75,7 +75,11 @@ type MpcParams # parameters for MPC solver Q_term_cost::Float64 delay_df::Int64 delay_a::Int64 - MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a) + Q_lane::Float64 # weight on the soft constraint for the lane + Q_vel::Float64 # weight on the soft constraint for the maximum velocity + + + MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0,Q_lane=1.0,Q_vel=1.0) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a,Q_lane,Q_vel) end type PosInfo # current position information @@ -91,7 +95,8 @@ type MpcSol # MPC solution output u::Array{Float64} z::Array{Float64} cost::Array{Float64} - MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost) + eps_alpha::Array{Float64} + MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[],eps_alpha=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost,eps_alpha) end type TrackCoeff # coefficients of track diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index e469c51c..2af34d1b 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -137,7 +137,7 @@ function main() # IMU measurements if i%2 == 0 # 50 Hz imu_drift = 1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) - rand_yaw = 0.01*randn() + rand_yaw = 0.05*randn() if rand_yaw > 0.1 rand_yaw = 0.1 elseif rand_yaw<-0.1 @@ -161,7 +161,7 @@ function main() imu_data.angular_velocity = Vector3(0,0,psiDot) imu_data.header.stamp = t_ros - rand_accX = 0.001*randn() + rand_accX = 0.01*randn() if rand_accX > 0.1 rand_accX = 0.1 elseif rand_accX<-0.1 @@ -170,7 +170,7 @@ function main() imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i,6]*z_current[i,4] #+rand_accX#+ randn()*0.3*1.0 - rand_accY = 0.001*randn() + rand_accY = 0.01*randn() if rand_accY > 0.1 rand_accY = 0.1 elseif rand_accY<-0.1 From 65929977e348a4182129468f5f603cf298b17a1f Mon Sep 17 00:00:00 2001 From: francescoricciuti Date: Tue, 14 Nov 2017 16:53:42 -0800 Subject: [PATCH 122/183] new plot functionalities --- eval_sim/eval_data.jl | 141 +++++---- workspace/src/barc/src/LMPC_node.jl | 23 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 279 +++++++++++++++++- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 4 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 8 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 50 +++- .../state_estimation_SensorKinematicModel.py | 2 +- 7 files changed, 422 insertions(+), 85 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index c83b7667..d1fd4da5 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -114,34 +114,50 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cvx = Data["cvx"] cvy = Data["cvy"] cpsi = Data["cpsi"] + input = Data["input"] + cost = Data["mpcCost"] + #status = Data["status"] - Nl = selectedStates.Nl - Np = selectedStates.Np + Nl = selectedStates.Nl + Np = selectedStates.Np + buffersize = 5000 + currentIt = lapStatus.currentIt + flag = zeros(2) - track = create_track(0.4) - + track = create_track(0.4) + for i = laps - vx_alpha = eps_alpha[1,1:200,i] - vy_alpha = eps_alpha[2,1:200,i] - psiDot_alpha = eps_alpha[3,1:200,i] - ePsi_alpha = eps_alpha[4,1:200,i] - eY_alpha = eps_alpha[5,1:200,i] - s_alpha = eps_alpha[6,1:200,i] - - cvx1 = cvx[1:200,1,i] - cvx2 = cvx[1:200,2,i] - cvx3 = cvx[1:200,3,i] - cvy1 = cvy[1:200,1,i] - cvy2 = cvy[1:200,2,i] - cvy3 = cvy[1:200,3,i] - cvy4 = cvy[1:200,4,i] - cpsi1 = cpsi[1:200,1,i] - cpsi2 = cpsi[1:200,2,i] - cpsi3 = cpsi[1:200,3,i] + # for index=1:buffersize + # if status[index,i] == :UserLimit + # flag[1]=index + # flag[2]= i + + # break + # end + # end + + + vx_alpha = eps_alpha[1,1:currentIt,i] + vy_alpha = eps_alpha[2,1:currentIt,i] + psiDot_alpha = eps_alpha[3,1:currentIt,i] + ePsi_alpha = eps_alpha[4,1:currentIt,i] + eY_alpha = eps_alpha[5,1:currentIt,i] + s_alpha = eps_alpha[6,1:currentIt,i] + + cvx1 = cvx[1:currentIt,1,i] + cvx2 = cvx[1:currentIt,2,i] + cvx3 = cvx[1:currentIt,3,i] + cvy1 = cvy[1:currentIt,1,i] + cvy2 = cvy[1:currentIt,2,i] + cvy3 = cvy[1:currentIt,3,i] + cvy4 = cvy[1:currentIt,4,i] + cpsi1 = cpsi[1:currentIt,1,i] + cpsi2 = cpsi[1:currentIt,2,i] + cpsi3 = cpsi[1:currentIt,3,i] figure(1) plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") @@ -151,31 +167,36 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) - t = linspace(1,200,200) + t = linspace(1,currentIt,currentIt) figure(2) subplot(221) - plot(t,one_step_error[1:200,1,i]) + plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # annotate("UserLimit",xy=[flag[1],one_step_error[flag[1],1,flag[2]]],xytext=[flag[1]+0.1,one_step_error[flag[1],1,flag[2]]+0.1],xycoords="data",arrowprops=["facecolor"=>"black"]) title("One step prediction error for v_x in lap $i") - ylim(-0.0001,findmax(one_step_error[1:200,1,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,1,i])[1]) + legend(["ospe","a_x","d_f"]) grid("on") subplot(222) - plot(t,one_step_error[1:200,2,i]) - ylim(-0.0001,findmax(one_step_error[1:200,2,i])[1]) + plot(t,one_step_error[1:currentIt,2,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,2,i])[1]) + legend(["ospe","a_x","d_f"]) title("One step prediction error for v_y in lap $i") grid("on") subplot(223) - plot(t,one_step_error[1:200,3,i]) - ylim(-0.0001,findmax(one_step_error[1:200,3,i])[1]) + plot(t,one_step_error[1:currentIt,3,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,3,i])[1]) + legend(["ospe","a_x","d_f"]) title("One step prediction error for psiDot in lap $i") grid("on") subplot(224) - plot(t,one_step_error[1:200,4,i]) - ylim(-0.0001,findmax(one_step_error[1:200,4,i])[1]) + plot(t,one_step_error[1:currentIt,4,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,4,i])[1]) + legend(["ospe","a_x","d_f"]) title("One step prediction error for ePsi in lap $i") grid("on") @@ -183,53 +204,60 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) figure(3) subplot(221) - plot(t,one_step_error[1:200,5,i]) - ylim(-0.0001,findmax(one_step_error[1:200,5,i])[1]) + plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,5,i])[1]) + legend(["ospe","a_x","d_f"]) title("One step prediction error for eY in lap $i") grid("on") subplot(222) - plot(t,one_step_error[1:200,6,i]) - ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + plot(t,one_step_error[1:currentIt,6,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + legend(["ospe","a_x","d_f"]) title("One step prediction error for s in lap $i") grid("on") + subplot(223) + plot(t,oldSS.oldSS[1:currentIt,6,i],"-*") + title("s in curren lap (to check repetitions)") + grid("on") + figure(4) subplot(221) plot(t,vx_alpha') - #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) title("Slack variable for vx in lap $i") grid("on") subplot(222) plot(t,vy_alpha') - #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) title("Slack variable for vy in lap $i") grid("on") subplot(223) plot(t,psiDot_alpha') - #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) title("Slack variable for psiDot in lap $i") grid("on") subplot(224) plot(t,ePsi_alpha') - #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) title("Slack variable for ePsi in lap $i") grid("on") figure(5) subplot(221) plot(t,eY_alpha') - #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) title("Slack variable for eY in lap $i") grid("on") subplot(222) plot(t,s_alpha') - #ylim(-0.0001,findmax(one_step_error[1:200,6,i])[1]) + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) title("Slack variable for s in lap $i") grid("on") @@ -253,6 +281,13 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("C_Psi in lap $i") grid("on") + figure(7) + plot(t,cost[1:currentIt,2,i],t,cost[1:currentIt,3,i],t,cost[1:currentIt,4,i],t,cost[1:currentIt,6,i]) + legend(["terminal Cost","control Cost","derivative Cost","lane Cost"]) + title("Costs of the Mpc") + grid("on") + + if switch == true @@ -271,34 +306,34 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) oldvx = selStates[1:Np,1,j,i] oldvx2 = selStates[Np+1:2*Np,1,j,i] - oldvx3 = selStates[2*Np+1:3*Np,1,j,i] + #oldvx3 = selStates[2*Np+1:3*Np,1,j,i] oldvy = selStates[1:Np,2,j,i] oldvy2 = selStates[Np+1:2*Np,2,j,i] - oldvy3 = selStates[2*Np+1:3*Np,2,j,i] + #oldvy3 = selStates[2*Np+1:3*Np,2,j,i] oldpsiDot = selStates[1:Np,3,j,i] oldpsiDot2 = selStates[Np+1:2*Np,3,j,i] - oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] + #oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] oldePsi = selStates[1:Np,4,j,i] oldePsi2 = selStates[Np+1:2*Np,4,j,i] - oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] + #oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] oldeY = selStates[1:Np,5,j,i] oldeY2 = selStates[Np+1:2*Np,5,j,i] - oldeY3 = selStates[2*Np+1:3*Np,5,j,i] + #oldeY3 = selStates[2*Np+1:3*Np,5,j,i] olds = selStates[1:Np,6,j,i] olds2 = selStates[Np+1:2*Np,6,j,i] - olds3 = selStates[2*Np+1:3*Np,6,j,i] + #olds3 = selStates[2*Np+1:3*Np,6,j,i] - figure(7) + figure(8) clf() subplot(221) plot(s_pred,vx_pred,"or") plot(olds,oldvx,"b") plot(olds2,oldvx2,"b") - plot(olds3,oldvx3,"b") + #plot(olds3,oldvx3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) title("State vx in lap $i, iteration $j") grid("on") @@ -307,7 +342,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,vy_pred,"or") plot(olds,oldvy,"b") plot(olds2,oldvy2,"b") - plot(olds3,oldvy3,"b") + #plot(olds3,oldvy3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) title("State vy in lap $i, iteration $j ") grid("on") @@ -316,7 +351,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,psiDot_pred,"or") plot(olds,oldpsiDot,"b") plot(olds2,oldpsiDot2,"b") - plot(olds3,oldpsiDot3,"b") + #plot(olds3,oldpsiDot3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) title("State psiDot in lap $i , iteration $j") grid("on") @@ -325,19 +360,19 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,ePsi_pred,"or") plot(olds,oldePsi,"b") plot(olds2,oldePsi2,"b") - plot(olds3,oldePsi3,"b") + #plot(olds3,oldePsi3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) title("State ePsi in lap $i, iteration $j ") grid("on") - figure(8) + figure(9) clf() subplot(221) plot(s_pred,eY_pred,"or") plot(olds,oldeY,"b") plot(olds2,oldeY2,"b") - plot(olds3,oldeY3,"b") + #plot(olds3,oldeY3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) title("State eY in lap $i, iteration $j ") grid("on") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 3a94982a..57ab418d 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -95,7 +95,8 @@ function main() mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) - mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) max_N = max(mpcParams.N,mpcParams_pF.N) @@ -135,6 +136,9 @@ function main() log_cvx = zeros(buffersize,3,30) log_cvy = zeros(buffersize,4,30) log_cpsi = zeros(buffersize,3,30) + log_input = zeros(buffersize,2,30) + log_status = Array(Symbol,buffersize,30) + log_mpcCost = zeros(buffersize,6,30) acc_f = [0.0] @@ -302,7 +306,7 @@ function main() setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,5],mpcSol.z[1:mpcParams.N+1,2]) setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,4],mpcSol.z[1:mpcParams.N+1,3]) setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) - setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + setvalue(mdl_convhull.alpha[:],1*ones(selectedStates.Nl*selectedStates.Np))#(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) elseif lapStatus.currentLap > n_pf+1 setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) @@ -317,7 +321,7 @@ function main() # ======================================= Calculate input ======================================= #println("*** NEW ITERATION # ",i," ***") - println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, ", s: ",posInfo.s) + println("Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt)#, ", s: ",posInfo.s) #println("State Nr. ", i, " = ", z_est) #println("s = $(posInfo.s)") #println("s_total = $(posInfo.s%posInfo.s_target)") @@ -355,8 +359,9 @@ function main() #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) zCurr[i,7] = acc0 #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) - solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) - + #solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + solveMpcProblem_test(mdl_test,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + acc0 = mpcSol.z[2,7] acc_f[1] = mpcSol.z[1,7] end @@ -370,6 +375,7 @@ function main() end log_t_solv[k+1] = toq() + #println("time= ",log_t_solv[k+1]) # Send command immediately, only if it is optimal! #if mpcSol.solverStatus == :Optimal @@ -404,6 +410,9 @@ function main() log_cvx[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Vx log_cvy[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Vy log_cpsi[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Psi + log_input[lapStatus.currentIt,:,lapStatus.currentLap] = uCurr[i,:] + log_mpcCost[lapStatus.currentIt,:,lapStatus.currentLap] = mpcSol.cost + #log_status[lapStatus.currentIt,lapStatus.currentLap] = mpcSol.solverStatus k = k + 1 # counter log_sol_status[k] = mpcSol.solverStatus @@ -450,7 +459,9 @@ function main() "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:], "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k],"selectedStates",selectedStates,"one_step_error",log_onestep, "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"lapStatus",lapStatus, - "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy) + "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy,"input",log_input, + "mpcCost",log_mpcCost) + #,"status",log_status) println("Exiting LMPC node. Saved data to $log_path.") println("number of same s in path following = ",same_sPF) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 42b3d049..830b084b 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -437,13 +437,13 @@ type MpcModel_convhull acc_f = 1.0 - mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.07))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) @variable( mdl, z_Ol[1:(N+1),1:7]) @variable( mdl, u_Ol[1:N,1:2]) @variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull - #@variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha + # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha #@variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity @@ -489,8 +489,8 @@ type MpcModel_convhull for n = 1:6 @NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np}) # terminal constraint - #@NLconstraint(mdl,z_Ol[N+1,i] >= sum{alpha[j]*selStates[j,i],j=1:Nl*Np}-eps_alpha[i]) - #@NLconstraint(mdl,z_Ol[N+1,i] <= sum{alpha[j]*selStates[j,i],j=1:Nl*Np}+eps_alpha[i]) + #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n]) + #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n]) end @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) @@ -522,12 +522,12 @@ type MpcModel_convhull @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s end - # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) - # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) - # for i=1:N-1 # Constraints on u: - # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) - # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) - # end + @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + end @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) @@ -570,7 +570,7 @@ type MpcModel_convhull # Overall Cost function (objective of the minimization) # ----------------------------------------------------- - @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost ) #+ slackCost)#+ velocityCost) + @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) @@ -605,8 +605,257 @@ type MpcModel_convhull end end -# why does the solution change when I start the simulation twice, totally independently? Is not everything initialized equally? -# why do I get a failed restoration phase even though there are *almost* no constraints and the solution should be super clear? -# does it actually make sense to use the first (initial) state as a @variable and combine it with a constraint to fix it to z0 ? -# or better use only a parameter for z0? -> Found out that solutions are different if I set constraints for first state or not!? + +type MpcModel_test + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,1} + c_Vy::Array{JuMP.NonlinearParameter,1} + c_Psi::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + + + #eps_lane::Array{JuMP.Variable,1} + #eps_alpha::Array{JuMP.Variable,1} + #eps_vel::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearExpression,1} + + derivCost::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + #laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + costPF::JuMP.NonlinearExpression + #slackCost::JuMP.NonlinearExpression + #velocityCost::JuMP.NonlinearExpression + + function MpcModel_test(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates) + + + m = new() + + #### Initialize parameters + + dt = modelParams.dt # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + u_lb = modelParams.u_lb # lower bounds for the control inputs + u_ub = modelParams.u_ub # upper bounds for the control inputs + z_lb = modelParams.z_lb # lower bounds for the states + z_ub = modelParams.z_ub # upper bounds for the states + c0 = modelParams.c0 + + + ey_max = trackCoeff.width/2 # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons + n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree for curvature approximation + v_max = 3 # maximum allowed velocity + + v_ref = mpcParams.vPathFollowing # reference velocity for the path following + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + #Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity + delay_df = mpcParams.delay_df + delay_a = mpcParams.delay_a + + + Np = selectedStates.Np::Int64 # how many states to select + Nl = selectedStates.Nl::Int64 # how many previous laps to select + + acc_f = 1.0 + + # Create function-specific parameters + z_Ref::Array{Float64,2} + z_Ref = cat(2,v_ref*ones(N+1,1),zeros(N+1,5)) # Reference trajectory: path following -> stay on line and keep constant velocity + u_Ref = zeros(N,2) + + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @variable( mdl, z_Ol[1:(N+1),1:7]) + @variable( mdl, u_Ol[1:N,1:2]) + #@variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha + #@variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity + + + + z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states + z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_6s[j,i]) + setupperbound(u_Ol[j,i], u_ub_6s[j,i]) + end + end + for i=1:7 + for j=1:N+1 + setlowerbound(z_Ol[j,i], z_lb_6s[j,i]) + setupperbound(z_Ol[j,i], z_ub_6s[j,i]) + end + end + + @NLparameter(mdl, z0[i=1:7] == 0) + @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0) + @NLparameter(mdl, c_Vx[i=1:3] == 0) + @NLparameter(mdl, c_Vy[i=1:4] == 0) + @NLparameter(mdl, c_Psi[i=1:3] == 0) + @NLparameter(mdl, uPrev[1:10,1:2] == 0) + @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" + + + # Conditions for first solve: + setvalue(z0[1],1) + setvalue(c_Vx[3],0.1) + + @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i]) + + #@NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) + #@NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) + #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity + @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull + + for i = 1:6 + @NLconstraint(mdl,z_Ol[N+1,i] == sum{alpha[j]*selStates[j,i],j=1:Nl*Np}) # terminal constraint + #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n]) + #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n]) + end + + @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) + @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i])) + + println("Initializing model...") + + # System dynamics + for i=1:N + if i<=delay_df + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2]) # psiDot + else + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2]) # psiDot + end + if i<=delay_a + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1])) + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f) + else + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f) + end + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7]) # xDot + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1])) # xDot + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7]) + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i])) # ePsi + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY + @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s + end + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end + + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + # end + + + + # Cost functions + + # Derivative cost + # --------------------------------- + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} + + QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ + QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) + + # Control Input cost + # --------------------------------- + @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+ + R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df}) + + # Lane cost (soft) + # --------------------------------- + #@NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) + + # Terminal Cost + # --------------------------------- + @NLexpression(mdl, terminalCost , sum{alpha[i]*statesCost[i], i=1:Nl*Np}) + + # Slack cost (soft) + # --------------------------------- + #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) + + # Velocity Cost + #---------------------------------- + #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) + + # Path Following Cost + #---------------------------------- + + @NLexpression(mdl, costPF, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:6}) # Follow trajectory + + + # Overall Cost function (objective of the minimization) + # ----------------------------------------------------- + + #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + @NLobjective(mdl, Min, derivCost + controlCost + costPF + terminalCost) + + + sol_stat=solve(mdl) + println("Finished solve 1: $sol_stat") + sol_stat=solve(mdl) + println("Finished solve 2: $sol_stat") + + + m.mdl = mdl + m.z0 = z0 + m.coeff = coeff + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.uPrev = uPrev + #m.eps_alpha=eps_alpha + + m.derivCost = derivCost + m.controlCost = controlCost + m.costPF = costPF + #m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + #m.velocityCost= velocityCost #velocity cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + + + return m + end +end + diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index f6d4af20..bf528fe2 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -103,7 +103,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,m=1:6] = oldSS.oldSS[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]+Np-(j*N_points2)-1,i=1:6,selected_laps[j+1]] # select the states from lap j... - selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = 0.7 * oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]] # and their cost + selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = 0.6 * oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]] # and their cost # if obstacle.lap_active == true # if the obstacles are on the track, check if any of the selected states interferes with the propagated obstacle @@ -192,7 +192,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo n_sys_ID_post = 60 # steps of sysID after current point in previous laps n_sys_ID_prev_c = 30 # steps of sysID before current point in current lap - freq_ratio = 4 # ratio between the frequency at which the informations about the position are updated in the topic and the frequency at which the controller calculates and updates the control input + freq_ratio = 5 # ratio between the frequency at which the informations about the position are updated in the topic and the frequency at which the controller calculates and updates the control input # vec_range_ID = () # for i=1:n_laps_sysID # vec_range_ID = tuple(vec_range_ID...,idx_s[i]-n_prev:idx_s[i]+n_ahead) # related index range diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 68225899..44adc7b2 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,14 +47,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 25 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.Nl = 3 # Number of previous laps to include in the convex hull + selectedStates.Np = 30 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) - mpcParams.N = 15 - mpcParams.Q = [5.0,0.0,0.0,1.0,10.0,0.0] # Q (only for path following mode) + mpcParams.N = 16 + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index bf5b9d09..e7d41d7e 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -81,8 +81,8 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M z_ref1 = cat(2,zeros(mpcParams.N+1,3),v_ref*ones(mpcParams.N+1,1)) - z_ref2 = cat(2,zeros(mpcParams.N+1,1),0.2*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) - z_ref3 = cat(2,zeros(mpcParams.N+1,1),-0.2*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) + z_ref2 = cat(2,zeros(mpcParams.N+1,1),0.1*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) + z_ref3 = cat(2,zeros(mpcParams.N+1,1),-0.1*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) sol_status::Symbol sol_u::Array{Float64,2} @@ -95,9 +95,9 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M if lapStatus.currentLap == 1 setvalue(mdl.z_Ref,z_ref1) elseif lapStatus.currentLap == 2 - setvalue(mdl.z_Ref,z_ref2) + setvalue(mdl.z_Ref,z_ref1) elseif lapStatus.currentLap == 3 - setvalue(mdl.z_Ref,z_ref3) + setvalue(mdl.z_Ref,z_ref1) end # Solve Problem and return solution @@ -165,3 +165,45 @@ function solveMpcProblem_convhull(m::MpcModel_convhull,mpcSol::MpcSol,mpcCoeff:: nothing end +function solveMpcProblem_test(m::MpcModel_test,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus, + posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},selectedStates::SelectedStates) + + # Load Parameters + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + selStates = selectedStates.selStates::Array{Float64,2} + statesCost = selectedStates.statesCost::Array{Float64,1} + + # Update current initial condition, curvature and System ID coefficients + setvalue(m.z0,zCurr) + setvalue(m.uPrev,uPrev) + setvalue(m.c_Vx,mpcCoeff.c_Vx) # System ID coefficients + setvalue(m.c_Vy,mpcCoeff.c_Vy) + setvalue(m.c_Psi,mpcCoeff.c_Psi) + setvalue(m.coeff,trackCoeff.coeffCurvature) # Track curvature + setvalue(m.selStates,selStates) + setvalue(m.statesCost,statesCost) + + # Solve Problem and return solution + sol_status = solve(m.mdl) + sol_u = getvalue(m.u_Ol) + sol_z = getvalue(m.z_Ol) + + # export data + mpcSol.a_x = sol_u[1,1] + mpcSol.d_f = sol_u[1,2] + mpcSol.u = sol_u + mpcSol.z = sol_z + #mpcSol.eps_alpha = getvalue(m.eps_alpha) + mpcSol.solverStatus = sol_status + mpcSol.cost = zeros(6) + #mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] + #println("o,terminal,control,deriv,0,lane= ",mpcSol.cost) + + println("Solved, status = $sol_status") + + nothing +end + diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 2642a471..57957331 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -178,7 +178,7 @@ def state_estimation(): vhMdl = (L_f, L_r) # set node rate - loop_rate = 40 + loop_rate = 50 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) se.t0 = rospy.get_rostime().to_sec() # set initial time From 486f304195df2c6d2f34d6b31b213648bf43d939 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 16 Nov 2017 10:03:00 -0800 Subject: [PATCH 123/183] added slack variables to the problem, new tuning, now working --- eval_sim/eval_data.jl | 89 +- workspace/src/barc/src/LMPC_node.jl | 83 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 76 +- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 109 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 6 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 5 +- workspace/src/barc/src/barc_lib/classes.jl | 9 +- workspace/src/barc/src/barc_record.jl | 2 - workspace/src/barc/src/barc_simulator_dyn.jl | 2 - workspace/src/barc/src/controller_MPC_traj.jl | 2 - workspace/src/barc/src/open_loop.jl | 2 - workspace/src/barc_gui/CMakeLists.txt | 193 --- workspace/src/barc_gui/license.txt | 21 - workspace/src/barc_gui/package.xml | 18 - workspace/src/barc_gui/plugin.xml | 20 - workspace/src/barc_gui/scripts/barc_gui | 10 - workspace/src/barc_gui/scripts/test.py | 23 - workspace/src/barc_gui/setup.py | 14 - .../barc_gui/src/barc_gui/BARC_experiment.ui | 92 -- .../src/barc_gui/src/barc_gui/__init__.py | 0 workspace/src/barc_gui/src/barc_gui/gui.py | 341 ----- workspace/src/barc_gui/src/barc_gui/test.py | 51 - workspace/src/data_service/CMakeLists.txt | 194 --- workspace/src/data_service/include/.gitignore | 0 workspace/src/data_service/license.txt | 21 - .../src/data_service/msg/CustomSignal.msg | 2 - workspace/src/data_service/msg/TimeData.msg | 2 - workspace/src/data_service/msg/TimeSeries.msg | 1 - workspace/src/data_service/msg/TimeSignal.msg | 3 - workspace/src/data_service/package.xml | 16 - workspace/src/data_service/scripts/.gitignore | 1 - workspace/src/data_service/scripts/base.py | 198 --- .../data_service/scripts/data_connection.py | 364 ----- .../src/data_service/scripts/raw_test.py | 34 - workspace/src/data_service/scripts/service.py | 208 --- workspace/src/data_service/scripts/test.py | 56 - workspace/src/data_service/scripts/upload.py | 103 -- .../src/data_service/srv/DataForward.srv | 5 - .../src/data_service/srv/DataRetrieve.srv | 4 - .../data_service/srv/RegisterExperiment.srv | 3 - .../src/data_service/srv/RegisterSetting.srv | 4 - .../src/data_service/srv/RegisterVideo.srv | 4 - workspace/src/rqt_common_plugins/.gitignore | 92 -- .../rqt_action/CHANGELOG.rst | 71 - .../rqt_action/CMakeLists.txt | 9 - .../rqt_common_plugins/rqt_action/package.xml | 34 - .../rqt_common_plugins/rqt_action/plugin.xml | 18 - .../rqt_common_plugins/rqt_action/setup.py | 12 - .../rqt_action/src/rqt_action/__init__.py | 0 .../src/rqt_action/action_plugin.py | 59 - .../rqt_common_plugins/rqt_bag/CHANGELOG.rst | 136 -- .../rqt_common_plugins/rqt_bag/CMakeLists.txt | 17 - .../rqt_common_plugins/rqt_bag/mainpage.dox | 7 - .../rqt_common_plugins/rqt_bag/package.xml | 31 - .../src/rqt_common_plugins/rqt_bag/plugin.xml | 17 - .../rqt_bag/resource/bag_widget.ui | 439 ------ .../rqt_bag/scripts/rqt_bag | 10 - .../src/rqt_common_plugins/rqt_bag/setup.py | 12 - .../rqt_bag/src/rqt_bag/__init__.py | 37 - .../rqt_bag/src/rqt_bag/bag.py | 99 -- .../rqt_bag/src/rqt_bag/bag_helper.py | 127 -- .../rqt_bag/src/rqt_bag/bag_timeline.py | 830 ------------ .../rqt_bag/src/rqt_bag/bag_widget.py | 354 ----- .../rqt_bag/src/rqt_bag/index_cache_thread.py | 86 -- .../src/rqt_bag/message_listener_thread.py | 86 -- .../src/rqt_bag/message_loader_thread.py | 112 -- .../rqt_bag/src/rqt_bag/player.py | 150 --- .../rqt_bag/src/rqt_bag/plugins/__init__.py | 0 .../src/rqt_bag/plugins/message_view.py | 95 -- .../rqt_bag/src/rqt_bag/plugins/plugin.py | 60 - .../rqt_bag/src/rqt_bag/plugins/raw_view.py | 231 ---- .../src/rqt_bag/plugins/timeline_renderer.py | 79 -- .../src/rqt_bag/plugins/topic_message_view.py | 103 -- .../rqt_bag/src/rqt_bag/recorder.py | 247 ---- .../rqt_bag/src/rqt_bag/timeline_cache.py | 177 --- .../rqt_bag/src/rqt_bag/timeline_frame.py | 1175 ----------------- .../rqt_bag/src/rqt_bag/timeline_menu.py | 275 ---- .../rqt_bag/src/rqt_bag/topic_selection.py | 112 -- .../rqt_bag_plugins/CHANGELOG.rst | 109 -- .../rqt_bag_plugins/CMakeLists.txt | 14 - .../rqt_bag_plugins/mainpage.dox | 14 - .../rqt_bag_plugins/package.xml | 36 - .../rqt_bag_plugins/plugin.xml | 13 - .../rqt_bag_plugins/resource/plot.ui | 162 --- .../rqt_bag_plugins/setup.py | 11 - .../src/rqt_bag_plugins/__init__.py | 0 .../src/rqt_bag_plugins/image_helper.py | 102 -- .../src/rqt_bag_plugins/image_plugin.py | 51 - .../image_timeline_renderer.py | 181 --- .../src/rqt_bag_plugins/image_view.py | 111 -- .../src/rqt_bag_plugins/plot_plugin.py | 50 - .../src/rqt_bag_plugins/plot_view.py | 495 ------- .../rqt_common_plugins/CHANGELOG.rst | 83 -- .../rqt_common_plugins/CMakeLists.txt | 4 - .../rqt_common_plugins/package.xml | 58 - .../rqt_console/CHANGELOG.rst | 103 -- .../rqt_console/CMakeLists.txt | 18 - .../rqt_console/mainpage.dox | 6 - .../rqt_console/package.xml | 27 - .../rqt_common_plugins/rqt_console/plugin.xml | 17 - .../resource/console_settings_dialog.ui | 120 -- .../rqt_console/resource/console_widget.ui | 340 ----- .../resource/filters/custom_filter_widget.ui | 188 --- .../resource/filters/filter_wrapper_widget.ui | 73 - .../resource/filters/list_filter_widget.ui | 56 - .../resource/filters/text_filter_widget.ui | 41 - .../resource/filters/time_filter_widget.ui | 74 -- .../resource/text_browse_dialog.ui | 67 - .../rqt_console/scripts/rqt_console | 8 - .../rqt_common_plugins/rqt_console/setup.py | 12 - .../rqt_console/src/rqt_console/__init__.py | 0 .../rqt_console/src/rqt_console/console.py | 139 -- .../rqt_console/console_settings_dialog.py | 73 - .../src/rqt_console/console_widget.py | 754 ----------- .../src/rqt_console/filters/__init__.py | 0 .../src/rqt_console/filters/base_filter.py | 77 -- .../src/rqt_console/filters/custom_filter.py | 93 -- .../filters/custom_filter_widget.py | 180 --- .../rqt_console/filters/filter_collection.py | 68 - .../filters/filter_wrapper_widget.py | 94 -- .../rqt_console/filters/list_filter_widget.py | 133 -- .../rqt_console/filters/location_filter.py | 45 - .../src/rqt_console/filters/message_filter.py | 104 -- .../src/rqt_console/filters/node_filter.py | 72 - .../rqt_console/filters/severity_filter.py | 74 -- .../rqt_console/filters/text_filter_widget.py | 97 -- .../src/rqt_console/filters/time_filter.py | 100 -- .../rqt_console/filters/time_filter_widget.py | 121 -- .../src/rqt_console/filters/topic_filter.py | 72 - .../rqt_console/src/rqt_console/message.py | 133 -- .../src/rqt_console/message_data_model.py | 267 ---- .../src/rqt_console/message_list.py | 63 - .../src/rqt_console/message_proxy_model.py | 132 -- .../src/rqt_console/text_browse_dialog.py | 50 - .../rqt_common_plugins/rqt_dep/CHANGELOG.rst | 111 -- .../rqt_common_plugins/rqt_dep/CMakeLists.txt | 22 - .../rqt_common_plugins/rqt_dep/mainpage.dox | 6 - .../rqt_common_plugins/rqt_dep/package.xml | 31 - .../src/rqt_common_plugins/rqt_dep/plugin.xml | 17 - .../rqt_dep/resource/RosPackGraph.ui | 274 ---- .../rqt_dep/scripts/rqt_dep | 8 - .../src/rqt_common_plugins/rqt_dep/setup.py | 12 - .../rqt_dep/src/rqt_dep/__init__.py | 0 .../rqt_dep/src/rqt_dep/dotcode_pack.py | 396 ------ .../rqt_dep/src/rqt_dep/ros_pack_graph.py | 417 ------ .../rqt_dep/test/dotcode_pack_test.py | 66 - .../rqt_graph/CHANGELOG.rst | 114 -- .../rqt_graph/CMakeLists.txt | 22 - .../rqt_common_plugins/rqt_graph/mainpage.dox | 6 - .../rqt_common_plugins/rqt_graph/package.xml | 41 - .../rqt_common_plugins/rqt_graph/plugin.xml | 17 - .../rqt_graph/resource/RosGraph.ui | 301 ----- .../rqt_graph/scripts/rqt_graph | 8 - .../src/rqt_common_plugins/rqt_graph/setup.py | 12 - .../rqt_graph/src/rqt_graph/__init__.py | 0 .../rqt_graph/src/rqt_graph/dotcode.py | 580 -------- .../rqt_graph/interactive_graphics_view.py | 103 -- .../rqt_graph/src/rqt_graph/ros_graph.py | 368 ------ .../rqt_graph/test/dotcode_test.py | 201 --- .../rqt_image_view/CHANGELOG.rst | 115 -- .../rqt_image_view/CMakeLists.txt | 66 - .../include/rqt_image_view/image_view.h | 116 -- .../rqt_image_view/ratio_layouted_frame.h | 102 -- .../rqt_image_view/mainpage.dox | 6 - .../rqt_image_view/package.xml | 31 - .../rqt_image_view/plugin.xml | 17 - .../rqt_image_view/scripts/rqt_image_view | 16 - .../rqt_image_view/setup.py | 11 - .../src/rqt_image_view/image_view.cpp | 371 ------ .../src/rqt_image_view/image_view.ui | 169 --- .../rqt_image_view/ratio_layouted_frame.cpp | 162 --- .../rqt_launch/CHANGELOG.rst | 75 -- .../rqt_launch/CMakeLists.txt | 18 - .../rqt_common_plugins/rqt_launch/package.xml | 32 - .../rqt_common_plugins/rqt_launch/plugin.xml | 18 - .../rqt_launch/resource/launch_widget.ui | 77 -- .../rqt_launch/resource/node_widget.ui | 108 -- .../rqt_launch/scripts/rqt_launch | 8 - .../rqt_common_plugins/rqt_launch/setup.py | 11 - .../rqt_launch/src/rqt_launch/__init__.py | 0 .../rqt_launch/src/rqt_launch/launch_main.py | 154 --- .../src/rqt_launch/launch_plugin.py | 64 - .../src/rqt_launch/launch_widget.py | 274 ---- .../src/rqt_launch/name_surrogate.py | 61 - .../src/rqt_launch/node_controller.py | 116 -- .../src/rqt_launch/node_delegate.py | 93 -- .../rqt_launch/src/rqt_launch/node_proxy.py | 125 -- .../rqt_launch/src/rqt_launch/node_widget.py | 119 -- .../src/rqt_launch/status_indicator.py | 40 - .../rqt_launch/test/test.launch | 7 - .../rqt_logger_level/CHANGELOG.rst | 103 -- .../rqt_logger_level/CMakeLists.txt | 18 - .../rqt_logger_level/mainpage.dox | 6 - .../rqt_logger_level/package.xml | 33 - .../rqt_logger_level/plugin.xml | 17 - .../rqt_logger_level/resource/logger_level.ui | 74 -- .../rqt_logger_level/scripts/rqt_logger_level | 8 - .../rqt_logger_level/setup.py | 12 - .../src/rqt_logger_level/__init__.py | 0 .../src/rqt_logger_level/logger_level.py | 70 - .../logger_level_service_caller.py | 126 -- .../rqt_logger_level/logger_level_widget.py | 126 -- .../rqt_common_plugins/rqt_msg/CHANGELOG.rst | 107 -- .../rqt_common_plugins/rqt_msg/CMakeLists.txt | 18 - .../rqt_common_plugins/rqt_msg/mainpage.dox | 7 - .../rqt_common_plugins/rqt_msg/package.xml | 33 - .../src/rqt_common_plugins/rqt_msg/plugin.xml | 17 - .../rqt_msg/resource/messages.ui | 124 -- .../rqt_msg/scripts/rqt_msg | 8 - .../src/rqt_common_plugins/rqt_msg/setup.py | 11 - .../rqt_msg/src/rqt_msg/__init__.py | 0 .../rqt_msg/src/rqt_msg/messages.py | 56 - .../src/rqt_msg/messages_tree_model.py | 42 - .../rqt_msg/src/rqt_msg/messages_tree_view.py | 47 - .../rqt_msg/src/rqt_msg/messages_widget.py | 216 --- .../rqt_common_plugins/rqt_plot/CHANGELOG.rst | 125 -- .../rqt_plot/CMakeLists.txt | 18 - .../rqt_common_plugins/rqt_plot/mainpage.dox | 6 - .../rqt_common_plugins/rqt_plot/package.xml | 32 - .../rqt_common_plugins/rqt_plot/plugin.xml | 17 - .../rqt_plot/resource/plot.ui | 157 --- .../rqt_plot/scripts/rqt_plot | 10 - .../src/rqt_common_plugins/rqt_plot/setup.py | 12 - .../rqt_plot/src/rqt_plot/__init__.py | 0 .../src/rqt_plot/data_plot/__init__.py | 526 -------- .../src/rqt_plot/data_plot/mat_data_plot.py | 155 --- .../rqt_plot/data_plot/pyqtgraph_data_plot.py | 111 -- .../src/rqt_plot/data_plot/qwt_data_plot.py | 255 ---- .../rqt_plot/src/rqt_plot/plot.py | 143 -- .../rqt_plot/src/rqt_plot/plot_widget.py | 315 ----- .../rqt_plot/src/rqt_plot/rosplot.py | 201 --- .../rqt_publisher/CHANGELOG.rst | 110 -- .../rqt_publisher/CMakeLists.txt | 18 - .../rqt_publisher/mainpage.dox | 6 - .../rqt_publisher/package.xml | 29 - .../rqt_publisher/plugin.xml | 17 - .../rqt_publisher/resource/Publisher.ui | 220 --- .../rqt_publisher/scripts/rqt_publisher | 8 - .../rqt_common_plugins/rqt_publisher/setup.py | 11 - .../src/rqt_publisher/__init__.py | 0 .../src/rqt_publisher/publisher.py | 342 ----- .../src/rqt_publisher/publisher_tree_model.py | 128 -- .../rqt_publisher/publisher_tree_widget.py | 73 - .../src/rqt_publisher/publisher_widget.py | 131 -- .../rqt_py_common/CHANGELOG.rst | 108 -- .../rqt_py_common/CMakeLists.txt | 26 - .../rqt_py_common/package.xml | 42 - .../resource/plugin_container.ui | 78 -- .../rqt_common_plugins/rqt_py_common/setup.py | 11 - .../src/rqt_py_common/__init__.py | 0 .../src/rqt_py_common/data_items.py | 45 - .../src/rqt_py_common/extended_combo_box.py | 105 -- .../src/rqt_py_common/ini_helper.py | 66 - .../src/rqt_py_common/item_delegates.py | 69 - .../src/rqt_py_common/layout_util.py | 103 -- .../src/rqt_py_common/message_tree_model.py | 137 -- .../src/rqt_py_common/message_tree_widget.py | 124 -- .../rqt_py_common/plugin_container_widget.py | 135 -- .../src/rqt_py_common/rosaction.py | 848 ------------ .../src/rqt_py_common/rqt_ros_graph.py | 163 --- .../src/rqt_py_common/rqt_roscomm_util.py | 215 --- .../src/rqt_py_common/topic_completer.py | 108 -- .../src/rqt_py_common/topic_dict.py | 66 - .../src/rqt_py_common/topic_helpers.py | 115 -- .../src/rqt_py_common/topic_tree_model.py | 49 - .../src/rqt_py_common/tree_model_completer.py | 48 - .../rqt_py_common/test/msg/ArrayVal.msg | 1 - .../rqt_py_common/test/msg/Val.msg | 1 - .../test/test_rqt_common_unit.py | 54 - .../rqt_py_common/test/test_rqt_ros_graph.py | 91 -- .../test/test_rqt_roscomm_util.py | 89 -- .../rqt_py_console/CHANGELOG.rst | 103 -- .../rqt_py_console/CMakeLists.txt | 18 - .../rqt_py_console/mainpage.dox | 7 - .../rqt_py_console/package.xml | 28 - .../rqt_py_console/plugin.xml | 17 - .../resource/py_console_widget.ui | 53 - .../rqt_py_console/scripts/rqt_py_console | 8 - .../rqt_py_console/setup.py | 11 - .../src/rqt_py_console/__init__.py | 0 .../src/rqt_py_console/py_console.py | 103 -- .../rqt_py_console/py_console_text_edit.py | 68 - .../src/rqt_py_console/py_console_widget.py | 54 - .../rqt_py_console/spyder_console_widget.py | 60 - .../rqt_reconfigure/CHANGELOG.rst | 125 -- .../rqt_reconfigure/CMakeLists.txt | 18 - .../rqt_reconfigure/package.xml | 38 - .../rqt_reconfigure/plugin.xml | 17 - .../rqt_reconfigure/resource/editor_bool.ui | 51 - .../rqt_reconfigure/resource/editor_enum.ui | 51 - .../rqt_reconfigure/resource/editor_number.ui | 108 -- .../rqt_reconfigure/resource/editor_string.ui | 51 - .../rqt_reconfigure/resource/node_selector.ui | 148 --- .../rqt_reconfigure/resource/param_main.ui | 58 - .../resource/param_timeline.ui | 31 - .../resource/paramedit_pane.ui | 59 - .../resource/singlenode_parameditor.ui | 484 ------- .../resource/text_filter_widget.ui | 32 - .../rqt_reconfigure/scripts/rqt_reconfigure | 10 - .../rqt_reconfigure/setup.py | 11 - .../src/rqt_reconfigure/__init__.py | 0 .../dynreconf_client_widget.py | 104 -- .../rqt_reconfigure/filter_children_model.py | 187 --- .../rqt_reconfigure/node_selector_widget.py | 477 ------- .../src/rqt_reconfigure/param_editors.py | 446 ------- .../src/rqt_reconfigure/param_groups.py | 329 ----- .../src/rqt_reconfigure/param_plugin.py | 71 - .../src/rqt_reconfigure/param_updater.py | 107 -- .../src/rqt_reconfigure/param_widget.py | 175 --- .../src/rqt_reconfigure/paramedit_widget.py | 167 --- .../src/rqt_reconfigure/text_filter.py | 106 -- .../src/rqt_reconfigure/text_filter_widget.py | 91 -- .../rqt_reconfigure/treenode_item_model.py | 77 -- .../src/rqt_reconfigure/treenode_qstditem.py | 265 ---- .../src/rqt_reconfigure/treenode_status.py | 89 -- .../rqt_reconfigure/test/__init__.py | 0 .../rqt_reconfigure/test/test_text_filter.py | 67 - .../test/test_treenode_qstditem.py | 69 - .../rqt_service_caller/CHANGELOG.rst | 104 -- .../rqt_service_caller/CMakeLists.txt | 18 - .../rqt_service_caller/mainpage.dox | 6 - .../rqt_service_caller/package.xml | 27 - .../rqt_service_caller/plugin.xml | 17 - .../resource/ServiceCaller.ui | 187 --- .../scripts/rqt_service_caller | 8 - .../rqt_service_caller/setup.py | 11 - .../src/rqt_service_caller/__init__.py | 0 .../src/rqt_service_caller/service_caller.py | 55 - .../service_caller_widget.py | 284 ---- .../rqt_shell/CHANGELOG.rst | 103 -- .../rqt_shell/CMakeLists.txt | 18 - .../rqt_common_plugins/rqt_shell/mainpage.dox | 7 - .../rqt_common_plugins/rqt_shell/package.xml | 27 - .../rqt_common_plugins/rqt_shell/plugin.xml | 17 - .../rqt_shell/resource/shell_widget.ui | 35 - .../rqt_shell/scripts/rqt_shell | 8 - .../src/rqt_common_plugins/rqt_shell/setup.py | 11 - .../rqt_shell/src/rqt_shell/__init__.py | 0 .../rqt_shell/src/rqt_shell/shell.py | 124 -- .../src/rqt_shell/shell_text_edit.py | 51 - .../rqt_shell/src/rqt_shell/shell_widget.py | 47 - .../src/rqt_shell/spyder_shell_widget.py | 121 -- .../rqt_shell/src/rqt_shell/xterm_widget.py | 71 - .../rqt_common_plugins/rqt_srv/CHANGELOG.rst | 104 -- .../rqt_common_plugins/rqt_srv/CMakeLists.txt | 14 - .../rqt_common_plugins/rqt_srv/mainpage.dox | 14 - .../rqt_common_plugins/rqt_srv/package.xml | 31 - .../src/rqt_common_plugins/rqt_srv/plugin.xml | 17 - .../rqt_srv/scripts/rqt_srv | 8 - .../src/rqt_common_plugins/rqt_srv/setup.py | 11 - .../rqt_srv/src/rqt_srv/__init__.py | 0 .../rqt_srv/src/rqt_srv/services.py | 60 - .../rqt_common_plugins/rqt_top/CHANGELOG.rst | 61 - .../rqt_common_plugins/rqt_top/CMakeLists.txt | 15 - .../rqt_common_plugins/rqt_top/mainpage.dox | 14 - .../rqt_common_plugins/rqt_top/package.xml | 26 - .../src/rqt_common_plugins/rqt_top/plugin.xml | 17 - .../rqt_top/scripts/rqt_top | 34 - .../src/rqt_common_plugins/rqt_top/setup.py | 11 - .../rqt_top/src/rqt_top/__init__.py | 0 .../rqt_top/src/rqt_top/node_info.py | 120 -- .../rqt_top/src/rqt_top/top_plugin.py | 192 --- .../rqt_topic/CHANGELOG.rst | 113 -- .../rqt_topic/CMakeLists.txt | 18 - .../rqt_common_plugins/rqt_topic/package.xml | 27 - .../rqt_common_plugins/rqt_topic/plugin.xml | 17 - .../rqt_topic/resource/TopicWidget.ui | 65 - .../rqt_topic/scripts/rqt_topic | 8 - .../src/rqt_common_plugins/rqt_topic/setup.py | 11 - .../rqt_topic/src/rqt_topic/__init__.py | 0 .../rqt_topic/src/rqt_topic/topic.py | 59 - .../rqt_topic/src/rqt_topic/topic_info.py | 130 -- .../rqt_topic/src/rqt_topic/topic_widget.py | 374 ------ .../rqt_common_plugins/rqt_web/CHANGELOG.rst | 104 -- .../rqt_common_plugins/rqt_web/CMakeLists.txt | 18 - .../rqt_common_plugins/rqt_web/mainpage.dox | 7 - .../rqt_common_plugins/rqt_web/package.xml | 27 - .../src/rqt_common_plugins/rqt_web/plugin.xml | 17 - .../rqt_web/resource/web_widget.ui | 59 - .../rqt_web/scripts/rqt_web | 8 - .../src/rqt_common_plugins/rqt_web/setup.py | 11 - .../rqt_web/src/rqt_web/__init__.py | 0 .../rqt_web/src/rqt_web/web.py | 72 - .../rqt_web/src/rqt_web/web_widget.py | 174 --- 384 files changed, 223 insertions(+), 34293 deletions(-) delete mode 100644 workspace/src/barc_gui/CMakeLists.txt delete mode 100644 workspace/src/barc_gui/license.txt delete mode 100644 workspace/src/barc_gui/package.xml delete mode 100644 workspace/src/barc_gui/plugin.xml delete mode 100755 workspace/src/barc_gui/scripts/barc_gui delete mode 100755 workspace/src/barc_gui/scripts/test.py delete mode 100755 workspace/src/barc_gui/setup.py delete mode 100755 workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui delete mode 100644 workspace/src/barc_gui/src/barc_gui/__init__.py delete mode 100644 workspace/src/barc_gui/src/barc_gui/gui.py delete mode 100644 workspace/src/barc_gui/src/barc_gui/test.py delete mode 100644 workspace/src/data_service/CMakeLists.txt delete mode 100644 workspace/src/data_service/include/.gitignore delete mode 100644 workspace/src/data_service/license.txt delete mode 100644 workspace/src/data_service/msg/CustomSignal.msg delete mode 100644 workspace/src/data_service/msg/TimeData.msg delete mode 100644 workspace/src/data_service/msg/TimeSeries.msg delete mode 100644 workspace/src/data_service/msg/TimeSignal.msg delete mode 100644 workspace/src/data_service/package.xml delete mode 100644 workspace/src/data_service/scripts/.gitignore delete mode 100644 workspace/src/data_service/scripts/base.py delete mode 100644 workspace/src/data_service/scripts/data_connection.py delete mode 100755 workspace/src/data_service/scripts/raw_test.py delete mode 100755 workspace/src/data_service/scripts/service.py delete mode 100755 workspace/src/data_service/scripts/test.py delete mode 100755 workspace/src/data_service/scripts/upload.py delete mode 100644 workspace/src/data_service/srv/DataForward.srv delete mode 100644 workspace/src/data_service/srv/DataRetrieve.srv delete mode 100644 workspace/src/data_service/srv/RegisterExperiment.srv delete mode 100644 workspace/src/data_service/srv/RegisterSetting.srv delete mode 100644 workspace/src/data_service/srv/RegisterVideo.srv delete mode 100644 workspace/src/rqt_common_plugins/.gitignore delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/__init__.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml delete mode 100755 workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_launch/test/test.launch delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_reconfigure/test/__init__.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py delete mode 100755 workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/plugin.xml delete mode 100755 workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/plugin.xml delete mode 100755 workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/mainpage.dox delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/package.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/plugin.xml delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui delete mode 100755 workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/setup.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/__init__.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py delete mode 100644 workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index d1fd4da5..35bada43 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -116,12 +116,13 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cpsi = Data["cpsi"] input = Data["input"] cost = Data["mpcCost"] + costSlack = Data["mpcCostSlack"] #status = Data["status"] Nl = selectedStates.Nl Np = selectedStates.Np buffersize = 5000 - currentIt = lapStatus.currentIt + currentIt = 200#lapStatus.currentIt flag = zeros(2) @@ -222,44 +223,44 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("s in curren lap (to check repetitions)") grid("on") - figure(4) - - subplot(221) - plot(t,vx_alpha') - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - title("Slack variable for vx in lap $i") - grid("on") - - subplot(222) - plot(t,vy_alpha') - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - title("Slack variable for vy in lap $i") - grid("on") - - subplot(223) - plot(t,psiDot_alpha') - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - title("Slack variable for psiDot in lap $i") - grid("on") - - subplot(224) - plot(t,ePsi_alpha') - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - title("Slack variable for ePsi in lap $i") - grid("on") - - figure(5) - subplot(221) - plot(t,eY_alpha') - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - title("Slack variable for eY in lap $i") - grid("on") - - subplot(222) - plot(t,s_alpha') - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - title("Slack variable for s in lap $i") - grid("on") + # figure(4) + + # subplot(221) + # plot(t,vx_alpha') + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # title("Slack variable for vx in lap $i") + # grid("on") + + # subplot(222) + # plot(t,vy_alpha') + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # title("Slack variable for vy in lap $i") + # grid("on") + + # subplot(223) + # plot(t,psiDot_alpha') + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # title("Slack variable for psiDot in lap $i") + # grid("on") + + # subplot(224) + # plot(t,ePsi_alpha') + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # title("Slack variable for ePsi in lap $i") + # grid("on") + + # figure(5) + # subplot(221) + # plot(t,eY_alpha') + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # title("Slack variable for eY in lap $i") + # grid("on") + + # subplot(222) + # plot(t,s_alpha') + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # title("Slack variable for s in lap $i") + # grid("on") figure(6) @@ -287,6 +288,12 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("Costs of the Mpc") grid("on") + figure(8) + plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i]) + legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + title("Slack costs") + grid("on") + if switch == true @@ -327,7 +334,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) - figure(8) + figure(9) clf() subplot(221) plot(s_pred,vx_pred,"or") @@ -366,7 +373,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) grid("on") - figure(9) + figure(10) clf() subplot(221) plot(s_pred,eY_pred,"or") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 57ab418d..d31fc10e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -2,11 +2,9 @@ using RobotOS @rosimport barc.msg: ECU, pos_info -@rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 rostypegen() using barc.msg -using data_service.msg using geometry_msgs.msg using JuMP using Ipopt @@ -96,7 +94,7 @@ function main() mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) - mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + #mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) max_N = max(mpcParams.N,mpcParams_pF.N) @@ -139,6 +137,8 @@ function main() log_input = zeros(buffersize,2,30) log_status = Array(Symbol,buffersize,30) log_mpcCost = zeros(buffersize,6,30) + log_mpcCostSlack = zeros(buffersize,6,30) + acc_f = [0.0] @@ -199,8 +199,8 @@ function main() posInfo.s = 0 - selectedStates.selStates = zeros(selectedStates.Nl*selectedStates.Np,6) - selectedStates.statesCost = zeros(selectedStates.Nl*selectedStates.Np) + #selectedStates.selStates = zeros(selectedStates.Nl*selectedStates.Np,6) + #selectedStates.statesCost = zeros(selectedStates.Nl*selectedStates.Np) oldSS.oldSS = NaN*ones(buffersize,7,30) @@ -251,12 +251,12 @@ function main() # ============================= Pre-Logging (before solving) ================================ log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) - if size(mpcSol.z,2) == 4 # find 1-step-error - step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]) - else - step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]) - end - log_step_diff[k+1,:] = step_diff + # if size(mpcSol.z,2) == 4 # find 1-step-error + # step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]) + # else + # step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]) + # end + # log_step_diff[k+1,:] = step_diff if lapStatus.currentLap > n_pf if lapStatus.currentIt>1 @@ -273,7 +273,7 @@ function main() cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target #save the terminal cost for j = 1:buffersize - cost2target[j] = (lapStatus.currentIt-j+1) # why do i need Q_cost? + cost2target[j] = 0.7*(lapStatus.currentIt-j+1) end oldSS.cost2target[:,lapStatus.currentLap-1] = cost2target @@ -295,22 +295,28 @@ function main() if lapStatus.currentLap <= n_pf setvalue(mdl_pF.z_Ol[:,1],mpcSol.z[:,1]-posInfo.s_target) elseif lapStatus.currentLap == n_pf+1 - setvalue(mdl.z_Ol[1:mpcParams.N,1],mpcSol.z[1:mpcParams.N,4]) - setvalue(mdl.z_Ol[1:mpcParams.N,6],mpcSol.z[1:mpcParams.N,1]-posInfo.s_target) - setvalue(mdl.z_Ol[1:mpcParams.N,5],mpcSol.z[1:mpcParams.N,2]) - setvalue(mdl.z_Ol[1:mpcParams.N,4],mpcSol.z[1:mpcParams.N,3]) - setvalue(mdl.u_Ol,mpcSol.u[1:mpcParams.N,:]) - - setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,1],mpcSol.z[1:mpcParams.N+1,4]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,6],mpcSol.z[1:mpcParams.N+1,1]-posInfo.s_target) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,5],mpcSol.z[1:mpcParams.N+1,2]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N+1,4],mpcSol.z[1:mpcParams.N+1,3]) - setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) - setvalue(mdl_convhull.alpha[:],1*ones(selectedStates.Nl*selectedStates.Np))#(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + if selectedStates.version == true + setvalue(mdl.z_Ol[1:mpcParams.N,1],mpcSol.z[1:mpcParams.N,4]) + setvalue(mdl.z_Ol[1:mpcParams.N,6],mpcSol.z[1:mpcParams.N,1]-posInfo.s_target) + setvalue(mdl.z_Ol[1:mpcParams.N,5],mpcSol.z[1:mpcParams.N,2]) + setvalue(mdl.z_Ol[1:mpcParams.N,4],mpcSol.z[1:mpcParams.N,3]) + setvalue(mdl.u_Ol,mpcSol.u[1:mpcParams.N,:]) + end + + setvalue(mdl_convhull.z_Ol[:,1],zCurr[1,1]*ones(mpcParams.N+1))#mpcSol.z[:,4]) + setvalue(mdl_convhull.z_Ol[:,6],zCurr[1,6]*ones(mpcParams.N+1))#mpcSol.z[:,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[:,5],zCurr[1,5]*ones(mpcParams.N+1))#mpcSol.z[:,2]) + setvalue(mdl_convhull.z_Ol[:,4],zCurr[1,4]*ones(mpcParams.N+1))#mpcSol.z[:,3]) + setvalue(mdl_convhull.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) + setvalue(mdl_convhull.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) + #setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) + setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) elseif lapStatus.currentLap > n_pf+1 - setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + if selectedStates.version == true + setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + end setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - #setvalue(mdl_convhull.alpha[1:selectedStates.Nl*selectedStates.Np],1/(selectedStates.Nl*selectedStates.Np)) + setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) end end @@ -358,21 +364,24 @@ function main() # otherwise: use system-ID-model #mpcCoeff.c_Vx[3] = max(mpcCoeff.c_Vx[3],0.1) zCurr[i,7] = acc0 - #solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) - #solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) - solveMpcProblem_test(mdl_test,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + if selectedStates.version == true + solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) + else + solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + #solveMpcProblem_test(mdl_test,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + end acc0 = mpcSol.z[2,7] acc_f[1] = mpcSol.z[1,7] end - if lapStatus.currentLap>n_pf + #if lapStatus.currentLap>n_pf # println("current s= ",posInfo.s) # println("current lap= ",lapStatus.currentLap) # println("selected states= ",selectedStates.selStates) # println("states cost= ",selectedStates.statesCost) # println("old safe set= ",oldSS.oldSS[lapStatus.currentIt:lapStatus.currentIt+10,6,lapStatus.currentLap-1]) # println("current control= ", [mpcSol.a_x,mpcSol.d_f]) - end + #end log_t_solv[k+1] = toq() #println("time= ",log_t_solv[k+1]) @@ -384,9 +393,9 @@ function main() #mpcSol.a_x = uPrev[1,1] #mpcSol.d_f = uPrev[1,2] #opt_count += 1 - if opt_count >= 5 - warn("No optimal solution for $opt_count iterations.") - end + # if opt_count >= 5 + # warn("No optimal solution for $opt_count iterations.") + # end #end #cmd.header.stamp = get_rostime() @@ -412,6 +421,10 @@ function main() log_cpsi[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Psi log_input[lapStatus.currentIt,:,lapStatus.currentLap] = uCurr[i,:] log_mpcCost[lapStatus.currentIt,:,lapStatus.currentLap] = mpcSol.cost + if lapStatus.currentLap > n_pf + log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack + end + #log_status[lapStatus.currentIt,lapStatus.currentLap] = mpcSol.solverStatus k = k + 1 # counter @@ -460,7 +473,7 @@ function main() "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k],"selectedStates",selectedStates,"one_step_error",log_onestep, "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"lapStatus",lapStatus, "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy,"input",log_input, - "mpcCost",log_mpcCost) + "mpcCost",log_mpcCost,"mpcCostSlack",log_mpcCostSlack) #,"status",log_status) println("Exiting LMPC node. Saved data to $log_path.") diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 830b084b..98014e14 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -395,6 +395,12 @@ type MpcModel_convhull controlCost::JuMP.NonlinearExpression laneCost::JuMP.NonlinearExpression terminalCost::JuMP.NonlinearExpression + slackVx::JuMP.NonlinearExpression + slackVy::JuMP.NonlinearExpression + slackPsidot::JuMP.NonlinearExpression + slackEpsi::JuMP.NonlinearExpression + slackEy::JuMP.NonlinearExpression + slackS::JuMP.NonlinearExpression #slackCost::JuMP.NonlinearExpression #velocityCost::JuMP.NonlinearExpression @@ -429,6 +435,7 @@ type MpcModel_convhull #Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity delay_df = mpcParams.delay_df delay_a = mpcParams.delay_a + Q_slack = mpcParams.Q_slack Np = selectedStates.Np::Int64 # how many states to select @@ -487,11 +494,11 @@ type MpcModel_convhull #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull - for n = 1:6 - @NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np}) # terminal constraint + #for n = 1:6 + #@NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np}) # terminal constraint #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n]) #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n]) - end + #end @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i])) @@ -522,19 +529,19 @@ type MpcModel_convhull @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s end - @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) - @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) - for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) - @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) - end + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) - for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) - end + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + # end @@ -563,6 +570,30 @@ type MpcModel_convhull # --------------------------------- #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) + # Slack cost on vx + #---------------------------------- + @NLexpression(mdl, slackVx, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1],j=1:Nl*Np})^2) + + # Slack cost on vy + #---------------------------------- + @NLexpression(mdl, slackVy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2],j=1:Nl*Np})^2) + + # Slack cost on Psi dot + #---------------------------------- + @NLexpression(mdl, slackPsidot, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3],j=1:Nl*Np})^2) + + # Slack cost on ePsi + #---------------------------------- + @NLexpression(mdl, slackEpsi, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4],j=1:Nl*Np})^2) + + # Slack cost on ey + #---------------------------------- + @NLexpression(mdl, slackEy, (z_Ol[N+1,5] - sum{alpha[j]*selStates[j,5],j=1:Nl*Np})^2) + + # Slack cost on s + #---------------------------------- + @NLexpression(mdl, slackS, (z_Ol[N+1,6] - sum{alpha[j]*selStates[j,6],j=1:Nl*Np})^2) + # Velocity Cost #---------------------------------- #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) @@ -570,14 +601,16 @@ type MpcModel_convhull # Overall Cost function (objective of the minimization) # ----------------------------------------------------- - @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + + @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS) sol_stat=solve(mdl) - println("Finished solve 1: $sol_stat") + println("Finished solve 1 convhull mpc: $sol_stat") sol_stat=solve(mdl) - println("Finished solve 2: $sol_stat") + println("Finished solve 2 convhull mpc: $sol_stat") m.mdl = mdl @@ -600,6 +633,13 @@ type MpcModel_convhull m.statesCost = statesCost # cost of the selected states m.alpha = alpha # parameters of the convex hull + m.slackVx = slackVx + m.slackVy = slackVy + m.slackPsidot = slackPsidot + m.slackEpsi = slackEpsi + m.slackEy = slackEy + m.slackS = slackS + return m end diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index bf528fe2..3c8acff1 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -93,7 +93,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! idx_s2= findmin(DistS2,1)[2] - off = 1 + off = 3 idx_s2 = idx_s2 + off @@ -103,7 +103,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,m=1:6] = oldSS.oldSS[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]+Np-(j*N_points2)-1,i=1:6,selected_laps[j+1]] # select the states from lap j... - selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = 0.6 * oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]] # and their cost + selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]] # and their cost # if obstacle.lap_active == true # if the obstacles are on the track, check if any of the selected states interferes with the propagated obstacle @@ -122,58 +122,59 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo end ########################################################################## - - vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) - - # Create the vectors used for the interpolation - # bS_vector contains the s-values for later interpolation - bS_Vector = zeros(pLength+1,2) - for i=1:pLength+1 - bS_Vector[i,1] = oldS[vec_range[1][i]] - bS_Vector[i,2] = oldS[vec_range[2][i]] - end - # if norm(bS_Vector[1,1]-s_total) > 0.3 || norm(bS_Vector[1,2]-s_total) > 0.3 - # warn("Couldn't find a close point to current s.") - # end - - # The states are parametrized with resprect to the curvilinear abscissa, - # so we select the point used for the interpolation. Need to subtract an - # offset to be coherent with the MPC formulation - s_forinterpy = bS_Vector - if s_total < 0 - s_forinterpy += s_target - end - - # Create the Matrices for the interpolation - MatrixInterp = zeros(pLength+1,Order+1,2) - - for k = 0:Order - MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k - end - - # Compute the coefficients - #coeffConst = zeros(Order+1,2,5) - for i=1:2 - mpcCoeff.coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldxDot[vec_range[i]] - mpcCoeff.coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldyDot[vec_range[i]] - mpcCoeff.coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldpsiDot[vec_range[i]] - mpcCoeff.coeffConst[:,i,4] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] - mpcCoeff.coeffConst[:,i,5] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] - end - - # Finished with calculating the constraint coefficients - - # Now compute the final cost coefficients - - # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value - # These values are calculated for both old trajectories - # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line - # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost - for i=1:2 - dist_to_s_target = oldTraj.oldCost[selected_laps[i]] + oldTraj.idx_start[selected_laps[i]] - (idx_s[i]-N_points*(i-1)) # number of iterations from idx_s to s_target - bQfunction_Vector = collect(linspace(dist_to_s_target,dist_to_s_target-pLength,pLength+1)) # build a vector that starts at the distance and - # decreases in equal steps - mpcCoeff.coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector # interpolate this vector with the given s + if selectedStates.version == true + vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) + + # Create the vectors used for the interpolation + # bS_vector contains the s-values for later interpolation + bS_Vector = zeros(pLength+1,2) + for i=1:pLength+1 + bS_Vector[i,1] = oldS[vec_range[1][i]] + bS_Vector[i,2] = oldS[vec_range[2][i]] + end + # if norm(bS_Vector[1,1]-s_total) > 0.3 || norm(bS_Vector[1,2]-s_total) > 0.3 + # warn("Couldn't find a close point to current s.") + # end + + # The states are parametrized with resprect to the curvilinear abscissa, + # so we select the point used for the interpolation. Need to subtract an + # offset to be coherent with the MPC formulation + s_forinterpy = bS_Vector + if s_total < 0 + s_forinterpy += s_target + end + + # Create the Matrices for the interpolation + MatrixInterp = zeros(pLength+1,Order+1,2) + + for k = 0:Order + MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k + end + + # Compute the coefficients + #coeffConst = zeros(Order+1,2,5) + for i=1:2 + mpcCoeff.coeffConst[:,i,1] = MatrixInterp[:,:,i]\oldxDot[vec_range[i]] + mpcCoeff.coeffConst[:,i,2] = MatrixInterp[:,:,i]\oldyDot[vec_range[i]] + mpcCoeff.coeffConst[:,i,3] = MatrixInterp[:,:,i]\oldpsiDot[vec_range[i]] + mpcCoeff.coeffConst[:,i,4] = MatrixInterp[:,:,i]\oldePsi[vec_range[i]] + mpcCoeff.coeffConst[:,i,5] = MatrixInterp[:,:,i]\oldeY[vec_range[i]] + end + + # Finished with calculating the constraint coefficients + + # Now compute the final cost coefficients + + # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value + # These values are calculated for both old trajectories + # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line + # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost + for i=1:2 + dist_to_s_target = oldTraj.oldCost[selected_laps[i]] + oldTraj.idx_start[selected_laps[i]] - (idx_s[i]-N_points*(i-1)) # number of iterations from idx_s to s_target + bQfunction_Vector = collect(linspace(dist_to_s_target,dist_to_s_target-pLength,pLength+1)) # build a vector that starts at the distance and + # decreases in equal steps + mpcCoeff.coeffCost[:,i] = MatrixInterp[:,:,i]\bQfunction_Vector # interpolate this vector with the given s + end end # --------------- SYSTEM IDENTIFICATION --------------- # diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 44adc7b2..ddc2a857 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,13 +47,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 30 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 16 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) + selectedStates.version = false - mpcParams.N = 16 + mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) @@ -65,6 +66,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + mpcParams.Q_slack = 10.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index e7d41d7e..ea36785d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -158,7 +158,10 @@ function solveMpcProblem_convhull(m::MpcModel_convhull,mpcSol::MpcSol,mpcCoeff:: mpcSol.solverStatus = sol_status mpcSol.cost = zeros(6) mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] - #println("o,terminal,control,deriv,0,lane= ",mpcSol.cost) + + mpcSol.costSlack = zeros(6) + mpcSol.costSlack = [getvalue(m.slackVx),getvalue(m.slackVy),getvalue(m.slackPsidot),getvalue(m.slackEpsi),getvalue(m.slackEy),getvalue(m.slackS)] + println("Solved, status = $sol_status") diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index e31cea21..e8ce1f41 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -59,7 +59,8 @@ type SelectedStates # Values needed for the convex hull formulat statesCost::Array{Float64} # ... and their related costs Np::Int64 # number of states to select from each previous lap Nl::Int64 # number of previous laps to include in the convex hull - SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2) = new(selStates,statesCost,Np,Nl) + version::Bool + SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false) = new(selStates,statesCost,Np,Nl,version) end type MpcParams # parameters for MPC solver @@ -77,9 +78,10 @@ type MpcParams # parameters for MPC solver delay_a::Int64 Q_lane::Float64 # weight on the soft constraint for the lane Q_vel::Float64 # weight on the soft constraint for the maximum velocity + Q_slack::Array{Float64,1} # weight on the slack variables for the terminal constraint - MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0,Q_lane=1.0,Q_vel=1.0) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a,Q_lane,Q_vel) + MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0,Q_lane=1.0,Q_vel=1.0,Q_slack=Float64[]) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a,Q_lane,Q_vel,Q_slack) end type PosInfo # current position information @@ -96,7 +98,8 @@ type MpcSol # MPC solution output z::Array{Float64} cost::Array{Float64} eps_alpha::Array{Float64} - MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[],eps_alpha=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost,eps_alpha) + costSlack::Array{Float64} + MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[],eps_alpha=Float64[],costSlack=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost,eps_alpha,costSlack) end type TrackCoeff # coefficients of track diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 976121e9..33e59ffe 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -11,13 +11,11 @@ Position info (/pos_info) using RobotOS @rosimport barc.msg: ECU, pos_info, Vel_est -@rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos rostypegen() using barc.msg -using data_service.msg using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 2af34d1b..8516ea0d 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -15,14 +15,12 @@ using RobotOS @rosimport barc.msg: ECU, Vel_est, pos_info -@rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos @rosimport std_msgs.msg: Header rostypegen() using barc.msg -using data_service.msg using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg diff --git a/workspace/src/barc/src/controller_MPC_traj.jl b/workspace/src/barc/src/controller_MPC_traj.jl index b8a32a26..2c203dc0 100755 --- a/workspace/src/barc/src/controller_MPC_traj.jl +++ b/workspace/src/barc/src/controller_MPC_traj.jl @@ -15,11 +15,9 @@ using RobotOS @rosimport barc.msg: ECU, pos_info, Encoder, Ultrasound, Z_KinBkMdl, Logging -@rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 rostypegen() using barc.msg -using data_service.msg using geometry_msgs.msg using JuMP using Ipopt diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index d549f04a..b41c3d94 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -2,13 +2,11 @@ using RobotOS @rosimport barc.msg: ECU, pos_info, Vel_est -@rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos rostypegen() using barc.msg -using data_service.msg using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg diff --git a/workspace/src/barc_gui/CMakeLists.txt b/workspace/src/barc_gui/CMakeLists.txt deleted file mode 100644 index 81332996..00000000 --- a/workspace/src/barc_gui/CMakeLists.txt +++ /dev/null @@ -1,193 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(barc_gui) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - rqt_gui - rqt_gui_py -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES rqt_car -# CATKIN_DEPENDS rospy rqt_gui rqt_gui_py -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(rqt_car -# src/${PROJECT_NAME}/rqt_car.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(rqt_car ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(rqt_car_node src/rqt_car_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(rqt_car_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(rqt_car_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - - -install(PROGRAMS scripts/barc_gui - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS rqt_car rqt_car_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_rqt_car.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/workspace/src/barc_gui/license.txt b/workspace/src/barc_gui/license.txt deleted file mode 100644 index a9651ed2..00000000 --- a/workspace/src/barc_gui/license.txt +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2016 Kiet Lam, Jon Gonzales - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/workspace/src/barc_gui/package.xml b/workspace/src/barc_gui/package.xml deleted file mode 100644 index e9f5b982..00000000 --- a/workspace/src/barc_gui/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - barc_gui - 0.0.0 - The BARC GUI package - mpc - MIT - catkin - rospy - rqt_gui - rqt_gui_py - rospy - rqt_gui - rqt_gui_py - - - - diff --git a/workspace/src/barc_gui/plugin.xml b/workspace/src/barc_gui/plugin.xml deleted file mode 100644 index 1910e9f7..00000000 --- a/workspace/src/barc_gui/plugin.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - system-help - UI - - - diff --git a/workspace/src/barc_gui/scripts/barc_gui b/workspace/src/barc_gui/scripts/barc_gui deleted file mode 100755 index c7b2b6fa..00000000 --- a/workspace/src/barc_gui/scripts/barc_gui +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main -from barc_gui.gui import MyGUI - -plugin = 'barc_gui' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin))#, plugin_argument_provider=Dot.add_arguments)) diff --git a/workspace/src/barc_gui/scripts/test.py b/workspace/src/barc_gui/scripts/test.py deleted file mode 100755 index 703ac06e..00000000 --- a/workspace/src/barc_gui/scripts/test.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -import rospy - -import random - -from std_msgs.msg import Float64 -from geometry_msgs.msg import Vector3 - -if __name__ == '__main__': - # Test rosbag - - rospy.init_node('barc_gui_test', anonymous=True) - - # pub = rospy.Publisher('barc_rosbag', Float64, queue_size=10) - pub = rospy.Publisher('enc_data', Vector3, queue_size=10) - # pub2 = rospy.Publisher('imu_data', Float64, queue_size=10) - rate = rospy.Rate(100) - - while not rospy.is_shutdown(): - pub.publish(Vector3(random.random(), random.random(), random.random())) - # pub2.publish(random.random()) - rate.sleep() diff --git a/workspace/src/barc_gui/setup.py b/workspace/src/barc_gui/setup.py deleted file mode 100755 index 1c20ba8d..00000000 --- a/workspace/src/barc_gui/setup.py +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - ## don't do this unless you want a globally visible script - # scripts=['bin/myscript'], - packages=['barc_gui'], - package_dir={'': 'src'}, - scripts=['scripts/barc_gui'] -) - -setup(**d) diff --git a/workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui b/workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui deleted file mode 100755 index a35a140b..00000000 --- a/workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui +++ /dev/null @@ -1,92 +0,0 @@ - - - MyGUI - - - - 0 - 0 - 436 - 351 - - - - - 0 - 0 - - - - Qt::ClickFocus - - - BARC Experiment - - - - - - Topics (check box for topic you want to record) - - - - - - - - - - - 0 - 0 - - - - Experiment name - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - - - - - - - 0 - 0 - - - - Start Recording - - - - - - - - - - diff --git a/workspace/src/barc_gui/src/barc_gui/__init__.py b/workspace/src/barc_gui/src/barc_gui/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/barc_gui/src/barc_gui/gui.py b/workspace/src/barc_gui/src/barc_gui/gui.py deleted file mode 100644 index e2d830d4..00000000 --- a/workspace/src/barc_gui/src/barc_gui/gui.py +++ /dev/null @@ -1,341 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import roslib -roslib.load_manifest('barc_gui') - -import subprocess, os, signal, psutil -import json - -import rospy -import rosbag - -from qt_gui.plugin import Plugin -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget - -import std_msgs.msg - -from PyQt4 import QtCore -from PyQt4.QtCore import * -from PyQt4.QtGui import * - -import time - -from data_service.srv import * -from data_service.msg import * - -from threading import * - - -rosbag_dir = os.path.expanduser("~") + '/rosbag' -video_dir = os.path.expanduser("~") + '/video' - - -class UploadThread(Thread): - - def __init__(self, plugin_ob, experiment): - Thread.__init__(self) - self.plugin_ob = plugin_ob - self.experiment = experiment - - - def run(self): - self.plugin_ob.record_data_thread(self.experiment) - - -class MyGUI(Plugin): - - def __init__(self, context): - super(MyGUI, self).__init__(context) - # Give QObjects reasonable names - self.setObjectName('MyGUI') - - # Process standalone plugin command-line arguments - from argparse import ArgumentParser - parser = ArgumentParser() - # Add argument(s) to the parser. - parser.add_argument("-q", "--quiet", action="store_true", - dest="quiet", - help="Put plugin in silent mode") - args, unknowns = parser.parse_known_args(context.argv()) - if not args.quiet: - print 'arguments: ', args - print 'unknowns: ', unknowns - - # Create QWidget - self._widget = QWidget() - # Get path to UI file which is a sibling of this file - # in this example the .ui and .py file are in the same folder - ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'BARC_experiment.ui') - # Extend the widget with all attributes and children from UI file - loadUi(ui_file, self._widget) - # Give QObjects reasonable names - self._widget.setObjectName('MyGUIUi') - # Show _widget.windowTitle on left-top of each plugin (when - # it's set in _widget). This is useful when you open multiple - # plugins at once. Also if you open multiple instances of your - # plugin at once, these lines add number to make it easy to - # tell from pane to pane. - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - # Add widget to the user interface - context.add_widget(self._widget) - - self._widget.pushbutton_record.clicked[bool].connect(self._handle_record) - self.record_started = False - - self.setup_topics_list() - self.p = None - - self.initialize() - - - def initialize(self): - if not os.path.isdir(video_dir): - os.makedirs(video_dir) - - if not os.path.isdir(rosbag_dir): - os.makedirs(rosbag_dir) - - - def get_experiment_name(self): - return self._widget.experiment_name.text().replace(' ', '_') - - - def setup_topics_list(self): - topics = ['imu', 'encoder', 'ecu', 'ultrasound', 'video'] - - for t in topics: - item = QListWidgetItem(t) - item.setCheckState(True) - - self._widget.listview_topics.addItem(item) - - - def start_record_video(self): - - print 'Recording video...' - - command = 'rosrun image_view video_recorder image:=/image_raw _max_depth_range:=0' - self.p_video = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=video_dir) - - - def stop_record_video(self, experiment): - print 'Stopping record video' - - if not self.p_video: - print 'Not stopping record video because p_video NONE' - return - - command = 'rosnode list' - out = subprocess.check_output(command, shell=True) - for li in out.split('\n'): - if 'video_recorder' in li: - command_kill = 'rosnode kill %s' % li - ps = subprocess.Popen(command_kill, stdin=subprocess.PIPE, shell=True) - ps.communicate() - - command = 'mv output.avi %s.avi' % experiment - subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=video_dir) - - self.p_video = None - - rospy.wait_for_service('register_video') - self.register_video = rospy.ServiceProxy('register_video', RegisterVideo) - - try: - self.register_video(experiment, video_dir + '/%s.avi' % experiment) - except Exception as e: - pass - - - def _handle_record(self, checked): - if not self.get_experiment_name(): - msg = QMessageBox() - msg.setText('Need an experiment name!') - msg.exec_() - return - - record_topics = [] - for index in range(self._widget.listview_topics.count()): - item = self._widget.listview_topics.item(index) - if item.checkState(): - record_topics.append(item.text()) - - if self.record_started: - self.stop_record_data(self.current_experiment) - self.stop_record_video(self.current_experiment) - - self.record_started = False - - self._widget.label_experiment.setText('Experiment name') - self._widget.pushbutton_record.setText('Start Recording') - - else: - self.record_started = True - self.current_experiment = self.get_experiment_name() - date = time.strftime("%Y.%m.%d") - self.current_experiment += '_' + date + '_' + time.strftime('%H.%M') - - self._widget.pushbutton_record.setText('Stop Recording') - self._widget.label_experiment.setText('Recording...') - self.time = time.time() - self.start_record_data(record_topics, self.current_experiment) - - if 'video' in record_topics: - self.start_record_video() - - - def start_record_data(self, topics, experiment): - command = 'rosbag record ' - - for topic in topics: - command += topic + ' ' - - command += ' -O %s' % experiment - - self.p = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=rosbag_dir) - - - def stop_record_data(self, experiment): - if not self.p: - return - - uploader = UploadThread(self, experiment) - uploader.start() - - - def record_data_thread(self, experiment): - command = 'rosnode list' - out = subprocess.check_output(command, shell=True) - - for li in out.split('\n'): - if 'record' in li: - command_kill = 'rosnode kill %s' % li - ps = subprocess.Popen(command_kill, stdin=subprocess.PIPE, shell=True) - ps.communicate() - - rospy.wait_for_service('send_data') - self.send_data = rospy.ServiceProxy('send_data', DataForward, persistent=True) - - self._widget.label_experiment.setText('Uploading data...') - self.upload_data(experiment) - self._widget.label_experiment.setText('Experiment name') - self._widget.pushbutton_record.setText('Start Recording') - - - def upload_data(self, experiment): - rosbag_file = os.path.abspath(rosbag_dir + '/' + experiment + '.bag') - - for i in range(100): - if os.path.isfile(rosbag_file): - break - #TODO: Can we get rid of this sleep? - time.sleep(1.0) - - if not os.path.isfile(rosbag_file): - return - - convert_to_time = 1/(10.0**(9)) - - bag = rosbag.Bag(rosbag_file) - - chunk_size = 50 - chunk_dict = dict() - chunk_msg = dict() - chunk_ts = dict() - - for topic, msg, t in bag.read_messages(): - ts = t.nsecs * convert_to_time - ts += (t.secs - self.time) - - if topic not in chunk_dict: - chunk_dict[topic] = 1 - - if topic not in chunk_msg: - chunk_msg[topic] = [] - - if topic not in chunk_ts: - chunk_ts[topic] = [] - - chunk_dict[topic] += 1 - chunk_msg[topic].append(msg) - chunk_ts[topic].append(ts) - - for k, v in chunk_dict.items(): - if v > chunk_size: - self.upload_message(k, chunk_msg[k], chunk_ts[k], experiment) - del chunk_msg[k] - del chunk_dict[k] - del chunk_ts[k] - - for k, v in chunk_dict.items(): - self.upload_message(k, chunk_msg[k], chunk_ts[k], experiment) - - os.remove(rosbag_file) - - - def upload_message(self, topic, msgs, tss, experiment): - vars_list = ['roll', 'pitch', 'yaw', - 'roll_rate', 'pitch_rate', 'yaw_rate', - 'acc_x', 'acc_y', 'acc_z', - 'encoder_FL', 'encoder_FR','encoder_BL','encoder_BR', - 'motor', 'servo', - 'ultrasound_front','ultrasound_back','ultrasound_left','ultrasound_right'] - - signal_dict = dict() - - for v in vars_list: - signal_dict[v] = [] - - for msg in msgs: - # Inertia measurement unit - if topic == 'imu': - (roll, pitch, yaw, acc_x, acc_y, acc_z, roll_rate, pitch_rate, yaw_rate) = msg.value - - # Encoder - if topic == 'encoder': - encoder_FL = msg.FL - encoder_FR = msg.FR - encoder_BL = msg.BL - encoder_BR = msg.BR - - if topic == 'ultrasound': - ultrasound_front = msg.front - ultrasound_back = msg.back - ultrasound_left = msg.left - ultrasound_right = msg.right - - # Electronic control unit - if topic == 'ecu': - motor = msg.motor - servo = msg.servo - - # Python introspection from list 'vars_list' - for v in vars_list: - if v in dict(globals(), **locals()): - signal_dict[v].append(dict(globals(), **locals())[v]) - - for v in vars_list: - if signal_dict[v]: - time_signal = TimeSignal() - time_signal.name = v - time_signal.timestamps = tss - time_signal.signal = json.dumps(signal_dict[v]) - try: - self.send_data(time_signal, None, experiment) - except Exception as e: - pass diff --git a/workspace/src/barc_gui/src/barc_gui/test.py b/workspace/src/barc_gui/src/barc_gui/test.py deleted file mode 100644 index 32310187..00000000 --- a/workspace/src/barc_gui/src/barc_gui/test.py +++ /dev/null @@ -1,51 +0,0 @@ -import sys -import urllib2 - -from PyQt4 import QtCore, QtGui - - -class DownloadThread(QtCore.QThread): - - data_downloaded = QtCore.pyqtSignal(object) - - def __init__(self, url): - QtCore.QThread.__init__(self) - self.url = url - - def run(self): - info = urllib2.urlopen(self.url).info() - self.data_downloaded.emit('%s\n%s' % (self.url, info)) - - -class MainWindow(QtGui.QWidget): - def __init__(self): - super(MainWindow, self).__init__() - self.list_widget = QtGui.QListWidget() - self.button = QtGui.QPushButton("Start") - self.button.clicked.connect(self.start_download) - layout = QtGui.QVBoxLayout() - layout.addWidget(self.button) - layout.addWidget(self.list_widget) - self.setLayout(layout) - - def start_download(self): - urls = ['http://google.com', 'http://twitter.com', 'http://yandex.ru', - 'http://stackoverflow.com/', 'http://www.youtube.com/'] - self.threads = [] - for url in urls: - downloader = DownloadThread(url) - downloader.data_downloaded.connect(self.on_data_ready) - self.threads.append(downloader) - downloader.start() - - def on_data_ready(self, data): - print data - self.list_widget.addItem(unicode(data)) - - -if __name__ == "__main__": - app = QtGui.QApplication(sys.argv) - window = MainWindow() - window.resize(640, 480) - window.show() - sys.exit(app.exec_()) diff --git a/workspace/src/data_service/CMakeLists.txt b/workspace/src/data_service/CMakeLists.txt deleted file mode 100644 index 0dbf885e..00000000 --- a/workspace/src/data_service/CMakeLists.txt +++ /dev/null @@ -1,194 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(data_service) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - genmsg - message_generation -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder - add_message_files( - FILES - CustomSignal.msg - TimeData.msg - TimeSeries.msg - TimeSignal.msg -) - -# Generate services in the 'srv' folder -add_service_files( - FILES - DataForward.srv - DataRetrieve.srv - RegisterExperiment.srv - RegisterSetting.srv - RegisterVideo.srv -) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES data_service - CATKIN_DEPENDS rospy std_msgs message_runtime -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(data_service -# src/${PROJECT_NAME}/data_service.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(data_service ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(data_service_node src/data_service_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(data_service_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(data_service_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS data_service data_service_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_data_service.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/workspace/src/data_service/include/.gitignore b/workspace/src/data_service/include/.gitignore deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/data_service/license.txt b/workspace/src/data_service/license.txt deleted file mode 100644 index 23422cde..00000000 --- a/workspace/src/data_service/license.txt +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2016 Kiet Lam - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/workspace/src/data_service/msg/CustomSignal.msg b/workspace/src/data_service/msg/CustomSignal.msg deleted file mode 100644 index dc97b3fa..00000000 --- a/workspace/src/data_service/msg/CustomSignal.msg +++ /dev/null @@ -1,2 +0,0 @@ -string id -string signal \ No newline at end of file diff --git a/workspace/src/data_service/msg/TimeData.msg b/workspace/src/data_service/msg/TimeData.msg deleted file mode 100644 index b742826f..00000000 --- a/workspace/src/data_service/msg/TimeData.msg +++ /dev/null @@ -1,2 +0,0 @@ -float64 timestamp -float64[] value diff --git a/workspace/src/data_service/msg/TimeSeries.msg b/workspace/src/data_service/msg/TimeSeries.msg deleted file mode 100644 index 619f8768..00000000 --- a/workspace/src/data_service/msg/TimeSeries.msg +++ /dev/null @@ -1 +0,0 @@ -TimeData[] series diff --git a/workspace/src/data_service/msg/TimeSignal.msg b/workspace/src/data_service/msg/TimeSignal.msg deleted file mode 100644 index 0b675da7..00000000 --- a/workspace/src/data_service/msg/TimeSignal.msg +++ /dev/null @@ -1,3 +0,0 @@ -string name -float64[] timestamps -string signal diff --git a/workspace/src/data_service/package.xml b/workspace/src/data_service/package.xml deleted file mode 100644 index 731a2466..00000000 --- a/workspace/src/data_service/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - data_service - 0.0.0 - The data_service package - mpc - MIT - message_generation - message_runtime - catkin - rospy - std_msgs - rospy - std_msgs - - diff --git a/workspace/src/data_service/scripts/.gitignore b/workspace/src/data_service/scripts/.gitignore deleted file mode 100644 index 36785238..00000000 --- a/workspace/src/data_service/scripts/.gitignore +++ /dev/null @@ -1 +0,0 @@ -default.cfg \ No newline at end of file diff --git a/workspace/src/data_service/scripts/base.py b/workspace/src/data_service/scripts/base.py deleted file mode 100644 index 346beadd..00000000 --- a/workspace/src/data_service/scripts/base.py +++ /dev/null @@ -1,198 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import json -import os -from uuid import uuid4 -import multiprocessing -import requests -import time -from data_connection import DataConnection - - -import os - - -class Configurator(object): - """ - Manages a config for the Local Computer - """ - - def __init__(self, **kwargs): - if "filename" in kwargs: - with open(kwargs["filename"], 'r') as input: - self.config = json.loads(input.read()) - else: - self.config = { - # ENVIRONMENT VARIBALE OR ROS ENVIRONMENT VARIABLE - 'name': os.environ['TEAM_NAME'], - 'server': os.environ['DATOR_SERVER'], - 'secret_uuid': str(uuid4()), - 'registration_token': 'abcd', - 'id': None - } - - def write_config(self, filename): - """ - Save current config to a json file. - """ - with open(filename, 'w') as output: - output.write(json.dumps(self.config, indent=4)) - - def get_config(self): - return self.config - - def set_config(self, config): - self.config = config - - -CONFIG_LOCATION = "/home/odroid/default.cfg" - - -def init_configurator(CONFIG_LOCATION=CONFIG_LOCATION): - """ Register local computer if not done previously. - :return: The configurator for this local computer - """ - if os.path.isfile(CONFIG_LOCATION): - configurator = Configurator(filename=CONFIG_LOCATION) - print "Found local configurator at {}".format(CONFIG_LOCATION) - else: - configurator = Configurator() - configurator.write_config(CONFIG_LOCATION) - - data_connection = DataConnection(configurator) - my_reg_token = str(uuid4()) - print "Registering to {} with token {}".format(configurator.get_config()['server'], my_reg_token) - data_connection.register(my_reg_token, CONFIG_LOCATION) - configurator.write_config(CONFIG_LOCATION) - - return configurator - - -def periodic_eval(refresh_time_sec, program, should_stop, shared_val, data_connection): - while not should_stop.value: - try: - eval(compile(program, '', 'exec')) - time.sleep(refresh_time_sec) - except BaseException as e: - print ("Error running uploaded program {}".format(e)) - if e.message: - print e.message - return periodic_eval - - -class WorkerPool(object): - - def __init__(self, data_connection): - self.job_list = {} - self.shared_val = multiprocessing.Value('i',0) - self.data_connection = data_connection - - def start_program(self, program_id, refresh_time_sec, program): - if program_id not in self.job_list.keys(): - should_stop = multiprocessing.Value('b', False) - self.job_list[program_id] = [should_stop, - multiprocessing.Process(target=periodic_eval, args=( - refresh_time_sec, - program, - should_stop, - self.shared_val, - self.data_connection - ))] - self.job_list[program_id][1].start() - else: - print "Program id {} already running".format(program_id) - - def stop_program(self, program_id): - try: - job = self.job_list[program_id] - self.job_list[program_id][0].value = True - self.job_list[program_id][1].join(20) - del self.job_list[program_id] - print "Stopped job for program id {}".format(program_id) - except BaseException as e: - print "Failed to stop program {}".format(program_id) - - def stop(self): - for program_id in self.job_list.keys(): - self.stop_program(program_id) - -COMMAND_NOOP = 0 -COMMAND_DONE = 1 -COMMAND_LOAD_PROGRAM = 2 -COMMAND_STOP_PROGRAM = 3 - - -class CommandHandler(object): - - def __init__(self, worker_pool, data_connection): - self.worker_pool = worker_pool - self.data_connection = data_connection - self.handler_map = { - COMMAND_LOAD_PROGRAM: self.handle_load, - COMMAND_STOP_PROGRAM: self.handle_stop, - } - - def handle_commands(self, commands): - done = False - for command in commands: - - print ("Rcx command {}".format(command["type"])) - if command['type'] == COMMAND_NOOP: - pass - elif command['type'] == COMMAND_DONE: - done = True - else: - self.handler_map[command['type']](command) - - data_connection.deactivate_command(command) - return done - - def handle_load(self, command): - program_command = json.loads(command['json_command']) - program = self.data_connection.get_program(program_command['program_id']) - print ("Starting program {}".format(program['id'])) - self.worker_pool.start_program(program['id'], program['sleep_time_sec'], program['code']) - - def handle_stop(self, command): - program_command = json.loads(command['json_command']) - program = self.data_connection.get_program(program_command['program_id']) - - self.worker_pool.stop_program(program['id']) - - -if __name__ == '__main__': - """ - Main loop. Handle commands until done. - """ - - configurator = init_configurator() - data_connection = DataConnection(configurator) - data_connection.update_config(CONFIG_LOCATION) - worker_pool = WorkerPool(data_connection) - command_handler = CommandHandler(worker_pool, data_connection) - - done = False - data_connection.set_local_computer_status(is_running=True) - while not done: - commands = data_connection.get_new_commands() - done = command_handler.handle_commands(commands) - time.sleep(configurator.get_config()['command_refresh_sec']) - - worker_pool.stop() - data_connection.set_local_computer_status(is_running=False) - print("got shared_value {}".format(worker_pool.shared_val.value)) - - print("Received done command. Shutting down.") diff --git a/workspace/src/data_service/scripts/data_connection.py b/workspace/src/data_service/scripts/data_connection.py deleted file mode 100644 index 4797cd40..00000000 --- a/workspace/src/data_service/scripts/data_connection.py +++ /dev/null @@ -1,364 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import json -import uuid -import requests -import datetime as tmz -import pytz -import delorean - - -class DataConnection(object): - - def _get_csrf(self): - """ - :return: Get the csrf from the local session - """ - response = self.client.get(url="{}/noop/".format(self.configurator.get_config()['server'])) - return response.cookies['csrftoken'] - - def __init__(self, configurator): - self.configurator = configurator - self.client = requests.session() - self.csrf = self._get_csrf() - - def _api_url(self, resource_name): - """ - Get restful endpoint for resource type - :param resource_name: The type of resource requested (e.g. "local_computer") - :return: The base api url to connect to the data server - """ - return "{}/api/v1/{}/?format=json".format(self.configurator.get_config()['server'], resource_name) - - def _item_url(self, resource_name, resource_id): - """ - Get restful endpoint for a specific instance of a resource. - :param resource_name: The type of resource requested - :param id: The id of the resource - :return: A specific resource instance - """ - return "{}/api/v1/{}/{}/?format=json".format(self.configurator.get_config()['server'], resource_name, - resource_id) - - def _data_item_url(self, resource_name, resource_id): - """ - Add data points to the given resource - :param resource_name: The type of resource requested - :param id: The id of the resource - :return: A specific data_api resource instance - """ - return "{}/data_api/v1/{}/{}/?format=json".format(self.configurator.get_config()['server'], - resource_name, resource_id) - - def register(self, registration_token=None, file_name=None): - """ - Call to register a new local computer. - """ - config = self.configurator.get_config() - if registration_token: - config["registration_token"] = registration_token - - config["secret_uuid"] = str(uuid.uuid4()) - - response = requests.post(self._api_url('local_computer'), - data=json.dumps(self.configurator.get_config()), - headers=self.post_header()) - - new_config = json.loads(response.content) - self.configurator.set_config(new_config) - - def update_config(self, config_location): - """ - :param config_location: Location of local config file - Update local config with global config. - """ - config = self.configurator.get_config() - url = self._item_url('local_computer', config['id']) - response = requests.get(url, headers= self.sec_header()) - if self.check_response_ok(response): - updated_config = json.loads(response.content) - for key in updated_config.keys(): - config[key] = updated_config[key] - self.configurator.write_config(config_location) - - def set_local_computer_status(self, is_running): - """ - :param isRunning: - """ - config = self.configurator.get_config() - config['is_running'] = is_running - url = self._item_url('local_computer', config['id']) - response = requests.put(url, data=json.dumps(self.configurator.get_config()), headers=self.sec_header()) - - def get_new_commands(self): - """ - :return: Commands from the server for this local computer that should be executed. - """ - config = self.configurator.get_config() - id = config["id"] - url = self._api_url('command') + "&is_local_computer_id={}&is_executed=false".format(id) - response = requests.get(url, headers = self.sec_header()) - if self.check_response_ok(response): - return json.loads(response.content)['objects'] - return [] - - def deactivate_command(self, command): - """ - Indicate that a command has been executed. - :param command: - :return: - """ - config = self.configurator.get_config() - id = config["id"] - command['is_executed'] = True - url = self._item_url('command', command['id']) + "&is_local_computer_id={}".format(id) - response = requests.put(url, data=json.dumps(command), headers=self.sec_header()) - self.check_response_ok(response) - - def get_program(self, program_id): - """ - :param program_id: - :return: the program object or None if it doesn't exist - """ - config = self.configurator.get_config() - url = self._item_url('program', program_id) - response = self.client.get(url, headers=self.sec_header()) - if self.check_response_ok(response): - return json.loads(response.content) - return None - - - def get_or_create_experiment(self, experiment_name): - """ - Get or create an experiment object for this local computer - :param experiment_name: - :return: The experiment object dict. - """ - config = self.configurator.get_config() - # print config - url = self._api_url('experiment') - params = {'name': experiment_name, 'local_computer_id': config['id']} - - response = self.client.get(url, params=params, headers=self.sec_header()) - - if len(json.loads(response.content)['objects']) == 0: - self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - - def add_signal_points(self, signal_id, signal_points): - """ Add sorted time/value points to a signal - :param signal_id: The id of the signal to add points to - :param signal_points: [[,],[, - """ - config = self.configurator.get_config() - url = self._data_item_url('signal', signal_id) - # print 'URL:', url - # print 'HEADERS:', self.post_header() - response = self.client.post(url, data=json.dumps(signal_points), headers=self.post_header()) - if not DataConnection.check_response_ok(response): - print "Error posting signal data {}".format(response.content) - print "Raising Exception!" - raise Exception("Error posting signal data {}".format(response.content)) - - def add_signal_points_by_name(self, signal_name, signal_points): - """ - Add time/value points to a signal object belonging to this local_computer. - :param signal_name: - :param signal_points: points in format[[,], ...] - """ - config = self.configurator.get_config() - - url = self._api_url('signal') - response = self.client.get(url, params={'name': signal_name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - signal_id = json.loads(response.content)['objects'][0]['id'] - self.add_signal_points(signal_id, signal_points) - - def get_signal_points(self, signal_id): - """ - Get the data points from the given signal. - :param signal_id: - :return: a list of signal point in format[[,], ...] - """ - config = self.configurator.get_config() - - url = self._data_item_url('signal', signal_id) - response = self.client.get(url,headers=self.sec_header()) - if self.check_response_ok(response): - return json.loads(response.content) - return [] - - def get_signal_points_by_name(self, name): - """ - Get the indicated signal from the local computer - :param name: The name of the signal - :return: a list of time/value points [[,] ...] - """ - config = self.configurator.get_config() - url = self._api_url('signal') - response = self.client.get(url, params={'name': name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - return self.get_signal_points(json.loads(response.content)['objects'][0]['id']) - - def get_or_create_signal(self, signal_name, experiment): - """ - Get or create a signal object for this local computer - :param signal_name: experiment: The experiment - :return: The signal object dict. - """ - config = self.configurator.get_config() - # print config - url = self._api_url('signal') - - params = {'name': signal_name, 'local_computer_id': config['id'], - 'experiment_id': experiment['id']} - - response = self.client.get(url, params=params, headers=self.sec_header()) - if len(json.loads(response.content)['objects']) == 0: - self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - def get_or_create_setting(self, key, experiment): - """ - Get or create a setting object for this local computer - :param setting: Setting key - :return: The setting object dict. - """ - config =self.configurator.get_config() - url = self._api_url('setting') - params = {'key': key, 'local_computer_id': config['id'], - 'experiment_id': experiment['id']} - - response = self.client.get(url, params=params, headers=self.sec_header()) - if len(json.loads(response.content)['objects']) == 0: - self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - def write_setting(self, value, setting_id): - config =self.configurator.get_config() - url = self._data_item_url('setting', setting_id) - - params = dict() - params['value'] = value - - response = self.client.post(url, data=json.dumps(params), headers=self.post_header()) - if not DataConnection.check_response_ok(response): - print "Error posting setting data {}".format(response.content) - print "Raising Exception!" - raise Exception("Error posting setting data {}".format(response.content)) - - - def get_or_create_blob(self, blob_name): - """ - Get or create a new blob object for this local computer - :param blob_name: - :return: the blob object dict. - """ - config =self.configurator.get_config() - url = self._api_url('blob') - params = {'name': blob_name, 'local_computer_id': config['id']} - response = self.client.get(url, params=params, headers=self.sec_header()) - if len(json.loads(response.content)['objects']) == 0: - response = self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - def set_blob_data(self, id, blob_data): - """ - Store data in a blob object. - :param id: - :param blob_data: String representation of blob to store - :return: - """ - url = self._data_item_url('blob', id) - response = self.client.post(url, data=blob_data, headers=self.post_header()) - if not DataConnection.check_response_ok(response): - print "Error posting blob data {}".format(response.content) - - def set_blob_data_by_name(self, name, blob_data): - config = self.configurator.get_config() - url = self._api_url('blob') - response = self.client.get(url, params={'name': name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - self.set_blob_data(json.loads(response.content)['objects'][0]['id'], blob_data) - - - def get_blob_data(self, id): - url = self._data_item_url('blob', id) - response = self.client.get(url, headers=self.sec_header()) - return response.content - - def get_blob_data_by_name(self, name): - config = self.configurator.get_config() - url = self._api_url('blob') - response = self.client.get(url, params={'name': name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - return self.get_blob_data(json.loads(response.content)['objects'][0]['id']) - - def create_event(self, event_type, info): - config = self.configurator.get_config() - url = self._api_url('event') - response = self.client.post(url, data=json.dumps({'type': event_type, 'info': info, - 'local_computer_id': config['id']}), - headers=self.post_header()) - if not self.check_response_ok(response): - print "Error creating event {}: {}".format(event_type, info) - - def sec_header(self, base_header=None): - auth_header = {'auth_key': self.configurator.get_config()["secret_uuid"], - 'content-type': 'application/json'} - if base_header is None: - return auth_header - auth_header.update(base_header) - - def post_header(self): - auth_header = {'auth_key': self.configurator.get_config()["secret_uuid"], - 'content-type': 'application/json', "X-CSRFToken": self.csrf} - return auth_header - - @classmethod - def check_response_ok(cls, response): - """ - :return: True if http response is a 200 class response. False info otherwise. - """ - if 200 <= response.status_code < 300: - return True - else: - response_string = "WARNING Command lookup failed: status: {} reason: {}".format(response.status_code, - response.reason) - if response.content: - response_string += response.content - print response_string - return False - - @classmethod - def millisec_to_utc(cls, millisec): - return tmz.datetime.fromtimestamp(float(millisec), tz=pytz.UTC) - - @classmethod - def utc_to_millisec(cls, dt): -# return delorean.Delorean(dt, timezone="UTC") -# print 'DELORRREAANN' -# print delorean.Delorean(dt, timezone="UTC").epoch - return delorean.Delorean(dt, timezone="UTC").epoch diff --git a/workspace/src/data_service/scripts/raw_test.py b/workspace/src/data_service/scripts/raw_test.py deleted file mode 100755 index efeb9f29..00000000 --- a/workspace/src/data_service/scripts/raw_test.py +++ /dev/null @@ -1,34 +0,0 @@ -#!/home/odroid/.virtualenvs/research/bin/python - -import time -from data_connection import * -from base import * -import datetime -import pytz - -# create data connection -configurator = init_configurator() -print configurator.config -data_connection = DataConnection(configurator) - -# signal = data_connection.get_or_create_signal("test_signal_saw_1") -signal = data_connection.get_or_create_signal("test_signal") - -# print signal - -# t = datetime.datetime.now(tz=pytz.UTC) -# print "got datetime {}".format(t) - -# tvec = [t + datetime.timedelta(seconds=i*.01) for i in range(10)] -# tsvec = [DataConnection.utc_to_millisec(at) for at in tvec] - -# # create a sawtooth wave -# signal_vector = [[ats-tsvec[0], ats] for ats in tsvec] - -# print signal_vector - -# data_connection.add_signal_points(signal['id'], signal_vector) - - -# Uncomment here to get data -print data_connection.get_signal_points(signal['id']) diff --git a/workspace/src/data_service/scripts/service.py b/workspace/src/data_service/scripts/service.py deleted file mode 100755 index a929c3cd..00000000 --- a/workspace/src/data_service/scripts/service.py +++ /dev/null @@ -1,208 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# ---------------------------------------------------------------------------#!/usr/bin/env python - -import time - -import rospy -import json -import datetime -import pytz - -from std_msgs.msg import Float64MultiArray - -from data_connection import * -from base import * - -from data_service.srv import * -from data_service.msg import * - - -configurator = init_configurator() -data_connection = DataConnection(configurator) - -signal_name_map = dict() -blob_name_map = dict() -experiment_name_map = dict() - -response_ok = 'Ok' - - -def get_experiment(experiment_name): - - if experiment_name not in experiment_name_map: - print 'Requesting new experiment' - experiment = data_connection.get_or_create_experiment(experiment_name) - experiment_name_map[experiment_name] = experiment - - return experiment_name_map[experiment_name] - - -def get_time_signal(signal_name, experiment_name): - map_key = (signal_name, experiment_name) - - experiment = get_experiment(experiment_name) - - if map_key not in signal_name_map: - print 'Requesting new time signal' - signal = data_connection.get_or_create_signal(signal_name, experiment) - signal_name_map[map_key] = signal - - return signal_name_map[map_key] - - -def get_blob_signal(signal_name): - - if signal_name not in blob_name_map: - print 'Requesting new blob signal' - signal = data_connection.get_or_create_blob(signal_name) - blob_name_map[signal_name] = signal - - return blob_name_map[signal_name] - - -def send_time_signal(time_signal, experiment_name): - signal_name = time_signal.name - signal = get_time_signal(signal_name, experiment_name) - response = response_ok - - timestamps = time_signal.timestamps - signals = json.loads(time_signal.signal) - signal_points = [] - - for ts, sig in zip(timestamps, signals): - if type(sig) is list: - signal_points.append([ts] + sig) - else: - signal_points.append([ts, sig]) - # sig.append(ts) - # signal_points.append(sig) - - data_connection.add_signal_points(signal['id'], - signal_points) - return response - - -def send_custom_signal(custom_signal, experiment_id): - signal = get_blob_signal(custom_signal.id) - response = response_ok - - try: - data_connection.set_blob_data(signal['id'], custom_signal.signal, experiment_id) - except Exception as e: - response = str(e) - - return response - - -def handle_send_data(req): - response = response_ok - - if req.time_signal != None and req.time_signal.name != '': - try: - send_time_signal(req.time_signal, req.experiment_name) - except Exception as e: - response = str(e) - print e - print 'Updating cache and retrying' - map_key = (req.time_signal.name, req.experiment_name) - if req.experiment_name in experiment_name_map: - del experiment_name_map[req.experiment_name] - if map_key in signal_name_map: - del signal_name_map[map_key] - send_time_signal(req.time_signal, req.experiment_name) - -# if req.custom_signal != None and req.custom_signal.id != '': -# response = send_custom_signal(req.custom_signal) -# if response != response_ok: -# return DataForwardResponse(response) - - return DataForwardResponse(response) - - -def handle_retrieve_data(req): - if req.id is None or req.id == '': - return None - - signal_id = req.id - - signal_response = CustomSignal() - signal_response.id = req.id - - if req.is_time: - signal = get_time_signal(signal_id) - try: - response = data_connection.get_signal_points(signal['id']) - signal_response.signal = json.dumps(response) - except Exception as e: - signal_response.signal = str(e) - return DataRetrieveResponse(signal_response) - else: - signal = get_blob_signal(signal_id) - try: - response = data_connection.get_blob_data(signal['id']) - signal_response.signal = response - except Exception as e: - signal_response.signal = str(e) - return DataRetrieveResponse(signal_response) - - return DataRetrieveResponse(signal_response) - - -def handle_register_experiment(req): - response = data_connection.get_or_create_experiment(req.experiment) - return RegisterExperimentResponse(response['id']) - - -def handle_register_setting(req): - response = response_ok - try: - setting = data_connection.get_or_create_setting(req.key) - data_connection.write_setting(req.key, req.value) - except Exception as e: - response = str(e) - - return RegisterSettingResponse(response) - - -def register_video(experiment, video_path): - try: - setting = data_connection.get_or_create_setting('video', experiment) - data_connection.write_setting(video_path, setting['id']) - return response_ok - except Exception as e: - return str(e) - - -def handle_register_video(req): - experiment = get_experiment(req.experiment) - response = register_video(experiment, req.path) - print response - return RegisterVideoResponse(response) - - -def send_data_service(): - rospy.init_node('data_service', anonymous=True) - s1 = rospy.Service('send_data', DataForward, handle_send_data) - s2 = rospy.Service('retrieve_data', DataRetrieve, handle_retrieve_data) - e1 = rospy.Service('register_experiment', RegisterExperiment, handle_register_experiment) - v1 = rospy.Service('register_video', RegisterVideo, handle_register_video) - - settings_service = rospy.Service('register_setting', RegisterSetting, handle_register_setting) - - rospy.spin() - - -if __name__ == "__main__": - send_data_service() diff --git a/workspace/src/data_service/scripts/test.py b/workspace/src/data_service/scripts/test.py deleted file mode 100755 index 8dd7bb10..00000000 --- a/workspace/src/data_service/scripts/test.py +++ /dev/null @@ -1,56 +0,0 @@ -#!/usr/bin/env python -import rospy - -import time -from data_connection import * -from base import * -import datetime -import pytz - -from data_service.srv import * -from data_service.msg import * - -from std_msgs.msg import Float64MultiArray - - -import json - -if __name__ == '__main__': - - rospy.wait_for_service('send_data') - rospy.wait_for_service('register_experiment') - rospy.wait_for_service('register_setting') - - experiment_name = 'ex3' - - # generate and send signal - try: - t = datetime.datetime.now(tz=pytz.UTC) - tvec = [t + datetime.timedelta(seconds=i*.01) for i in range(10)] - tsvec = [DataConnection.utc_to_millisec(at) for at in tvec] - # signal_vector = [ats-tsvec[0] for ats in tsvec] - signal_vector = [float(i)*15.3 for i in range(len(tsvec))] - signal_vs = [] - - for s in signal_vector: - signal_vs.append([s, 3]) - - send_data = rospy.ServiceProxy('send_data', DataForward) - time_signal = TimeSignal() - time_signal.name = 'mpc_sig2' - time_signal.timestamps = signal_vector - time_signal.signal = json.dumps(signal_vs) - - print "Time Signal", time_signal - print "Experiment name", experiment_name - - response = send_data(time_signal, None, experiment_name) - except rospy.ServiceException, e: - print 'Call to service failed: %s' %e - - - # # Register a setting - register_setting = rospy.ServiceProxy('register_setting', RegisterSetting) - - register_setting('I', '0.20') - register_setting('P', '0.60') diff --git a/workspace/src/data_service/scripts/upload.py b/workspace/src/data_service/scripts/upload.py deleted file mode 100755 index 3a1eece9..00000000 --- a/workspace/src/data_service/scripts/upload.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import os, sys -import time - -os.chdir('/home/odroid/barc/workspace/src/data_service/scripts') - -from data_connection import * -from base import * - -proj_path = '/home/odroid/barc/Dator' - -os.environ.setdefault('DJANGO_SETTINGS_MODULE', 'dator.settings') -sys.path.append(proj_path) -os.chdir(proj_path) - -from django.core.wsgi import get_wsgi_application -application = get_wsgi_application() - -from data_api.models import * - -import boto3 - -CLOUD_CONFIG_LOCATION = '/home/odroid/cloud.cfg' - - -S3_VIDEOS_BUCKET = 'datorvideos' - -rosbag_dir = os.path.expanduser("~") + '/rosbag' -video_dir = os.path.expanduser("~") + '/video' - - -if __name__ == '__main__': - configurator = init_configurator(CLOUD_CONFIG_LOCATION) - data_connection = DataConnection(configurator) - - s3_client = boto3.client('s3') - s3 = boto3.resource('s3') - - for sig in Signal.objects.all(): - try: - experiment = data_connection.get_or_create_experiment(sig.experiment.name) - - for setting in sig.experiment.setting_set.all(): - setting_remote = data_connection.get_or_create_setting(setting.key, experiment) - - #TODO: Check if AWS S3 token exists - if setting.key == 'video': - if setting.value.startswith(video_dir): - key_name = '%s_%s.avi' % (os.environ['TEAM_NAME'], sig.experiment.name) - - bucket = s3.Bucket(S3_VIDEOS_BUCKET) - bucket.Acl().put(ACL='public-read') - - obj = s3.Object(S3_VIDEOS_BUCKET, key_name) - print 'Uploading video' - video_path = '%s/%s.avi' %(video_dir, sig.experiment.name) - obj.put(Body=open(video_path, 'rb')) - obj.Acl().put(ACL='public-read') - - print 'Finished uploading video' - url = '{}/{}/{}'.format(s3_client.meta.endpoint_url, - S3_VIDEOS_BUCKET, key_name) - setting.value = url - setting.save() - - os.remove(video_path) - - data_connection.write_setting(setting.value, setting_remote['id']) - - signal = data_connection.get_or_create_signal(sig.name, experiment) - - try: - lst = LocalSignalTag.objects.filter(signal__name=sig.name, signal__experiment__name=sig.experiment.name)[0] - except (LocalSignalTag.DoesNotExist, IndexError) as e: - lst = LocalSignalTag() - lst.signal = sig - lst.uploaded = False - - if not lst.uploaded: - print 'Uploading' - data_connection.add_signal_points(signal['id'], sig.get_data()) - lst.uploaded = True - lst.save() - print 'Finished Uploading' - - lst.save() - - except Exception as e: - print e diff --git a/workspace/src/data_service/srv/DataForward.srv b/workspace/src/data_service/srv/DataForward.srv deleted file mode 100644 index 7cf208ca..00000000 --- a/workspace/src/data_service/srv/DataForward.srv +++ /dev/null @@ -1,5 +0,0 @@ -TimeSignal time_signal -CustomSignal custom_signal -string experiment_name ---- -string response diff --git a/workspace/src/data_service/srv/DataRetrieve.srv b/workspace/src/data_service/srv/DataRetrieve.srv deleted file mode 100644 index 916122d0..00000000 --- a/workspace/src/data_service/srv/DataRetrieve.srv +++ /dev/null @@ -1,4 +0,0 @@ -string id -bool is_time ---- -CustomSignal signal \ No newline at end of file diff --git a/workspace/src/data_service/srv/RegisterExperiment.srv b/workspace/src/data_service/srv/RegisterExperiment.srv deleted file mode 100644 index 1842fe63..00000000 --- a/workspace/src/data_service/srv/RegisterExperiment.srv +++ /dev/null @@ -1,3 +0,0 @@ -string experiment ---- -int32 experiment_id \ No newline at end of file diff --git a/workspace/src/data_service/srv/RegisterSetting.srv b/workspace/src/data_service/srv/RegisterSetting.srv deleted file mode 100644 index 54e669b6..00000000 --- a/workspace/src/data_service/srv/RegisterSetting.srv +++ /dev/null @@ -1,4 +0,0 @@ -string key -string value ---- -string response \ No newline at end of file diff --git a/workspace/src/data_service/srv/RegisterVideo.srv b/workspace/src/data_service/srv/RegisterVideo.srv deleted file mode 100644 index 35a01246..00000000 --- a/workspace/src/data_service/srv/RegisterVideo.srv +++ /dev/null @@ -1,4 +0,0 @@ -string experiment -string path ---- -string response \ No newline at end of file diff --git a/workspace/src/rqt_common_plugins/.gitignore b/workspace/src/rqt_common_plugins/.gitignore deleted file mode 100644 index 64b3b480..00000000 --- a/workspace/src/rqt_common_plugins/.gitignore +++ /dev/null @@ -1,92 +0,0 @@ -*.project -*.pydevproject -*.cproject - -# Created by https://www.gitignore.io - -### Python ### -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] - -# C extensions -*.so - -# Distribution / packaging -.Python -env/ -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -*.egg-info/ -.installed.cfg -*.egg - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.coverage -.cache -nosetests.xml -coverage.xml - -# Translations -*.mo -*.pot - -# Django stuff: -*.log - -# Sphinx documentation -docs/_build/ - -# PyBuilder -target/ - - -### C++ ### -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app diff --git a/workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst deleted file mode 100644 index af24d752..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst +++ /dev/null @@ -1,71 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_action -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Now depends on rqt_msg to eliminate GUI files from this package -* Fix; IndexError: list index out of range (`#26 `_) -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt deleted file mode 100644 index 0a2ab5e7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_action) -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_action/package.xml b/workspace/src/rqt_common_plugins/rqt_action/package.xml deleted file mode 100644 index 2d4f5682..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - rqt_action - 0.3.13 - rqt_action provides a feature to introspect all available ROS - action (from actionlib) types. By utilizing rqt_msg, the output format is - unified with it and rqt_srv. Note that the actions shown on this plugin - is the ones that are stored on your machine, not on the ROS core your rqt - instance connects to. - - Aaron Blasdel - BSD - - - http://ros.org/wiki/rqt_action - https://github.com/ros-visualization/rqt_common_plugins/rqt_action - https://github.com/ros-visualization/rqt_common_plugins/issues - - Isaac Isao Saito - - catkin - - - - rospy - rqt_msg - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_action/plugin.xml b/workspace/src/rqt_common_plugins/rqt_action/plugin.xml deleted file mode 100644 index 0ed78daa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/plugin.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - A Python GUI for browsing available ROS action types. - - - - - folder - Plugins related to ROS actions. - - - mail-read - A Python GUI for browsing available ROS action types. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_action/setup.py b/workspace/src/rqt_common_plugins/rqt_action/setup.py deleted file mode 100644 index f18d1442..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_action'], - package_dir={'': 'src'}, -# scripts=['scripts/rqt_action'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/__init__.py b/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py b/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py deleted file mode 100644 index 12b0e060..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py +++ /dev/null @@ -1,59 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin - -from rqt_msg.messages_widget import MessagesWidget -from rqt_py_common import rosaction - - -class ActionPlugin(Plugin): - def __init__(self, context): - super(ActionPlugin, self).__init__(context) - self.setObjectName('Action') - self._widget = MessagesWidget(rosaction.MODE_ACTION) - self._widget.setWindowTitle('Action Type Browser') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # v = instance_settings.value(k) - pass diff --git a/workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst deleted file mode 100644 index b86d771e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst +++ /dev/null @@ -1,136 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_bag -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* RQT_BAG: Ensure monotonic clock publishing. - Due to parallelism issues, a message can be published - with a simulated timestamp in the past. This lead to - undesired behaviors when using TF for example. -* Contributors: lsouchet - -0.3.12 (2015-07-24) -------------------- -* Added step-by-step playback capability -* Contributors: Aaron Blasdel, sambrose - -0.3.11 (2015-04-30) -------------------- -* fix viewer plugin relocation issue (`#306 `_) - -0.3.10 (2014-10-01) -------------------- -* fix topic type retrieval for multiple bag files (`#279 `_) -* fix region_changed signal emission when no start/end stamps are set -* improve right-click menu -* improve popup management (`#280 `_) -* implement recording of topic subsets -* sort the list of topics -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- -* fix visibility with dark Qt theme (`#263 `_) - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix compatibility with Groovy, use queue_size for Python publishers only when available (`#243 `_) -* use thread for loading bag files, emit region changed signal used by plotting plugin (`#239 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* fix closing and reopening topic views -* use queue_size for Python publishers - -0.3.5 (2014-05-07) ------------------- -* fix raw view not showing fields named 'msg' (`#226 `_) - -0.3.4 (2014-01-28) ------------------- -* add option to publish clock tim from bag (`#204 `_) - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* fix high cpu load when idle (`#194 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* update rqt_bag plugin interface to work with qt_gui_core 0.2.18 - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) -* fix shutdown of plugin (`#31 `_) -* fix saving parts of a bag (`#96 `_) -* fix long topic names (`#114 `_) -* fix zoom behavior (`#76 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- -* Fix; skips time when resuming playback (`#5 `_) -* Fix; timestamp printing issue (`#6 `_) - -0.2.8 (2013-01-11) ------------------- -* expose command line arguments to rqt_bag script -* added fix to set play/pause button correctly when fastforwarding/rewinding, adjusted time headers to 0m00s instead of 0:00m for ease of reading -* support passing bagfiles on the command line (currently behind --args) - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into Groovy diff --git a/workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt deleted file mode 100644 index 8855342d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_bag) -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_bag - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox deleted file mode 100644 index f4005088..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_bag provides a GUI plugin for displaying and replaying ROS bag files. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_bag/package.xml b/workspace/src/rqt_common_plugins/rqt_bag/package.xml deleted file mode 100644 index 9192c18b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_bag - 0.3.13 - rqt_bag provides a GUI plugin for displaying and replaying ROS bag files. - Aaron Blasdel - Austin Hendrix - - BSD - - http://ros.org/wiki/rqt_bag - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - Tim Field - - catkin - - python-rospkg - rosbag - rosgraph_msgs - roslib - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag/plugin.xml b/workspace/src/rqt_common_plugins/rqt_bag/plugin.xml deleted file mode 100644 index 8ae63b9c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for displaying and replaying ROS bag files. - - - - - folder - Plugins related to logging. - - - applications-other - A Python GUI plugin for displaying and replaying ROS bag files. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui b/workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui deleted file mode 100644 index 8aa56770..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui +++ /dev/null @@ -1,439 +0,0 @@ - - - Bag - - - - 0 - 0 - 943 - 325 - - - - Bag - - - - 0 - - - 0 - - - - - - - Record Bag - - - - - - - 16 - 16 - - - - - - - - Load Bag - - - - - - - 16 - 16 - - - - - - - - Save Bag - - - - - - - 16 - 16 - - - - - - - - Qt::Vertical - - - - - - - Go To Beginning - - - - - - - 16 - 16 - - - - - - - - Slower / Rewind - - - - - - - 16 - 16 - - - - - - - - Previous frame - - - - - - - 16 - 16 - - - - - - - - Play / Pause - - - - - - - 16 - 16 - - - - true - - - - - - - Next frame - - - - - - - 16 - 16 - - - - - - - - Fast Forward - - - - - - - 16 - 16 - - - - - - - - Go To End - - - - - - - 16 - 16 - - - - - - - - Qt::Vertical - - - - - - - Zoom In - - - - - - - 16 - 16 - - - - - - - - Zoom Out - - - - - - - 16 - 16 - - - - - - - - Zoom Home - - - - - - - 16 - 16 - - - - - - - - Qt::Vertical - - - - - - - Toggle Thumbnails - - - - - - - 16 - 16 - - - - true - - - - - - - Qt::Vertical - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::ScrollBarAlwaysOff - - - - - - - - - - 0 - 0 - - - - 0 - - - false - - - - - - - - 125 - 16777215 - - - - Time Stamp of the current playhead - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - 180 - 16777215 - - - - Date and time of the playhead - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - 0 - 0 - - - - - 110 - 16777215 - - - - Number of Seconds from the beginning of the current playhead - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - 80 - 16777215 - - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - - - BagGraphicsView - QGraphicsView -

rqt_bag.bag_widget
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag b/workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag deleted file mode 100755 index c769a7a5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_bag.bag import Bag -from rqt_gui.main import Main - -plugin = 'rqt_bag.bag.Bag' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin, plugin_argument_provider=Bag.add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/setup.py b/workspace/src/rqt_common_plugins/rqt_bag/setup.py deleted file mode 100644 index ee444b82..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_bag', 'rqt_bag.plugins'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_bag'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py deleted file mode 100644 index 1e8f644a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py +++ /dev/null @@ -1,37 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import bag_helper -from plugins.message_view import MessageView -from plugins.topic_message_view import TopicMessageView -from plugins.timeline_renderer import TimelineRenderer -from timeline_cache import TimelineCache diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py deleted file mode 100644 index be26e620..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py +++ /dev/null @@ -1,99 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import argparse -import threading - -from qt_gui.plugin import Plugin - -from .bag_widget import BagWidget - -class Bag(Plugin): - """ - Subclass of Plugin to provide interactive bag visualization, playing(publishing) and recording - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(Bag, self).__init__(context) - self.setObjectName('Bag') - - args = self._parse_args(context.argv()) - - self._widget = BagWidget(context, args.clock) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def load_bags(): - for bagfile in args.bagfiles: - self._widget.load_bag(bagfile) - - load_thread = threading.Thread(target=load_bags) - load_thread.start() - - def _parse_args(self, argv): - parser = argparse.ArgumentParser(prog='rqt_bag', add_help=False) - Bag.add_arguments(parser) - return parser.parse_args(argv) - - @staticmethod - def _isfile(parser, arg): - if os.path.isfile(arg): - return arg - else: - parser.error("Bag file %s does not exist" % ( arg )) - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_bag plugin') - group.add_argument('--clock', action='store_true', help='publish the clock time') - group.add_argument('bagfiles', type=lambda x: Bag._isfile(parser, x), - nargs='*', default=[], help='Bagfiles to load') - - def shutdown_plugin(self): - self._widget.shutdown_all() - - def save_settings(self, plugin_settings, instance_settings): - # TODO implement saving - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # TODO implement restoring - # v = instance_settings.value(k) - pass - - #def trigger_configuration(self): - # TODO move some of the button functionality to config button if it is "more configy" diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py deleted file mode 100644 index c7ef7b87..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py +++ /dev/null @@ -1,127 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICTS -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Helper functions for bag files and timestamps. -""" - -import time -import rospy - - -def stamp_to_str(t): - """ - Convert a rospy.Time to a human-readable string. - - @param t: time to convert - @type t: rospy.Time - """ - t_sec = t.to_sec() - if t < rospy.Time.from_sec(60 * 60 * 24 * 365 * 5): - # Display timestamps earlier than 1975 as seconds - return '%.3fs' % t_sec - else: - return time.strftime('%b %d %Y %H:%M:%S', time.localtime(t_sec)) + '.%03d' % (t.nsecs / 1000000) - - -def get_topics(bag): - """ - Get an alphabetical list of all the unique topics in the bag. - - @return: sorted list of topics - @rtype: list of str - """ - return sorted(set([c.topic for c in bag._get_connections()])) - - -def get_start_stamp(bag): - """ - Get the earliest timestamp in the bag. - - @param bag: bag file - @type bag: rosbag.Bag - @return: earliest timestamp - @rtype: rospy.Time - """ - start_stamp = None - for connection_start_stamp in [index[0].time for index in bag._connection_indexes.values()]: - if not start_stamp or connection_start_stamp < start_stamp: - start_stamp = connection_start_stamp - return start_stamp - - -def get_end_stamp(bag): - """ - Get the latest timestamp in the bag. - - @param bag: bag file - @type bag: rosbag.Bag - @return: latest timestamp - @rtype: rospy.Time - """ - end_stamp = None - for connection_end_stamp in [index[-1].time for index in bag._connection_indexes.values()]: - if not end_stamp or connection_end_stamp > end_stamp: - end_stamp = connection_end_stamp - - return end_stamp - - -def get_topics_by_datatype(bag): - """ - Get all the message types in the bag and their associated topics. - - @param bag: bag file - @type bag: rosbag.Bag - @return: mapping from message typename to list of topics - @rtype: dict of str to list of str - """ - topics_by_datatype = {} - for c in bag._get_connections(): - topics_by_datatype.setdefault(c.datatype, []).append(c.topic) - - return topics_by_datatype - - -def get_datatype(bag, topic): - """ - Get the datatype of the given topic. - - @param bag: bag file - @type bag: rosbag.Bag - @return: message typename - @rtype: str - """ - for c in bag._get_connections(topic): - return c.datatype - - return None diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py deleted file mode 100644 index 2722097c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py +++ /dev/null @@ -1,830 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy -import rosbag -import time -import threading - - -from python_qt_binding.QtCore import Qt, QTimer, qWarning, Signal -from python_qt_binding.QtGui import QGraphicsScene, QMessageBox - -import bag_helper - -from .timeline_frame import TimelineFrame -from .message_listener_thread import MessageListenerThread -from .message_loader_thread import MessageLoaderThread -from .player import Player -from .recorder import Recorder -from .timeline_menu import TimelinePopupMenu - - -class BagTimeline(QGraphicsScene): - """ - BagTimeline contains bag files, all information required to display the bag data visualization on the screen - Also handles events - """ - status_bar_changed_signal = Signal() - selected_region_changed = Signal(rospy.Time, rospy.Time) - - def __init__(self, context, publish_clock): - """ - :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext'' - """ - super(BagTimeline, self).__init__() - self._bags = [] - self._bag_lock = threading.RLock() - - self.background_task = None # Display string - self.background_task_cancel = False - - # Playing / Recording - self._playhead_lock = threading.RLock() - self._max_play_speed = 1024.0 # fastest X play speed - self._min_play_speed = 1.0 / 1024.0 # slowest X play speed - self._play_speed = 0.0 - self._play_all = False - self._playhead_positions_cvs = {} - self._playhead_positions = {} # topic -> (bag, position) - self._message_loaders = {} - self._messages_cvs = {} - self._messages = {} # topic -> (bag, msg_data) - self._message_listener_threads = {} # listener -> MessageListenerThread - self._player = False - self._publish_clock = publish_clock - self._recorder = None - self.last_frame = None - self.last_playhead = None - self.desired_playhead = None - self.wrap = True # should the playhead wrap when it reaches the end? - self.stick_to_end = False # should the playhead stick to the end? - self._play_timer = QTimer() - self._play_timer.timeout.connect(self.on_idle) - self._play_timer.setInterval(3) - - # Plugin popup management - self._context = context - self.popups = {} - self._views = [] - self._listeners = {} - - # Initialize scene - # the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast. - # otherwise a dark qt theme will default it to black and the frame render pen will be unreadable - self.setBackgroundBrush(Qt.white) - self._timeline_frame = TimelineFrame(self) - self._timeline_frame.setPos(0, 0) - self.addItem(self._timeline_frame) - - self.background_progress = 0 - self.__closed = False - - def get_context(self): - """ - :returns: the ROS_GUI context, 'PluginContext' - """ - return self._context - - def handle_close(self): - """ - Cleans up the timeline, bag and any threads - """ - if self.__closed: - return - else: - self.__closed = True - self._play_timer.stop() - for topic in self._get_topics(): - self.stop_publishing(topic) - self._message_loaders[topic].stop() - if self._player: - self._player.stop() - if self._recorder: - self._recorder.stop() - if self.background_task is not None: - self.background_task_cancel = True - self._timeline_frame.handle_close() - for bag in self._bags: - bag.close() - for frame in self._views: - if frame.parent(): - self._context.remove_widget(frame) - - # Bag Management and access - def add_bag(self, bag): - """ - creates an indexing thread for each new topic in the bag - fixes the boarders and notifies the indexing thread to index the new items bags - :param bag: ros bag file, ''rosbag.bag'' - """ - self._bags.append(bag) - - bag_topics = bag_helper.get_topics(bag) - - new_topics = set(bag_topics) - set(self._timeline_frame.topics) - - for topic in new_topics: - self._playhead_positions_cvs[topic] = threading.Condition() - self._messages_cvs[topic] = threading.Condition() - self._message_loaders[topic] = MessageLoaderThread(self, topic) - - self._timeline_frame._start_stamp = self._get_start_stamp() - self._timeline_frame._end_stamp = self._get_end_stamp() - self._timeline_frame.topics = self._get_topics() - self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype() - # If this is the first bag, reset the timeline - if self._timeline_frame._stamp_left is None: - self._timeline_frame.reset_timeline() - - # Invalidate entire index cache for all topics in this bag - with self._timeline_frame.index_cache_cv: - for topic in bag_topics: - self._timeline_frame.invalidated_caches.add(topic) - if topic in self._timeline_frame.index_cache: - del self._timeline_frame.index_cache[topic] - - self._timeline_frame.index_cache_cv.notify() - - #TODO Rethink API and if these need to be visible - def _get_start_stamp(self): - """ - :return: first stamp in the bags, ''rospy.Time'' - """ - with self._bag_lock: - start_stamp = None - for bag in self._bags: - bag_start_stamp = bag_helper.get_start_stamp(bag) - if bag_start_stamp is not None and (start_stamp is None or bag_start_stamp < start_stamp): - start_stamp = bag_start_stamp - return start_stamp - - def _get_end_stamp(self): - """ - :return: last stamp in the bags, ''rospy.Time'' - """ - with self._bag_lock: - end_stamp = None - for bag in self._bags: - bag_end_stamp = bag_helper.get_end_stamp(bag) - if bag_end_stamp is not None and (end_stamp is None or bag_end_stamp > end_stamp): - end_stamp = bag_end_stamp - return end_stamp - - def _get_topics(self): - """ - :return: sorted list of topic names, ''list(str)'' - """ - with self._bag_lock: - topics = set() - for bag in self._bags: - for topic in bag_helper.get_topics(bag): - topics.add(topic) - return sorted(topics) - - def _get_topics_by_datatype(self): - """ - :return: dict of list of topics for each datatype, ''dict(datatype:list(topic))'' - """ - with self._bag_lock: - topics_by_datatype = {} - for bag in self._bags: - for datatype, topics in bag_helper.get_topics_by_datatype(bag).items(): - topics_by_datatype.setdefault(datatype, []).extend(topics) - return topics_by_datatype - - def get_datatype(self, topic): - """ - :return: datatype associated with a topic, ''str'' - :raises: if there are multiple datatypes assigned to a single topic, ''Exception'' - """ - with self._bag_lock: - datatype = None - for bag in self._bags: - bag_datatype = bag_helper.get_datatype(bag, topic) - if datatype and bag_datatype and (bag_datatype != datatype): - raise Exception('topic %s has multiple datatypes: %s and %s' % (topic, datatype, bag_datatype)) - if bag_datatype: - datatype = bag_datatype - return datatype - - def get_entries(self, topics, start_stamp, end_stamp): - """ - generator function for bag entries - :param topics: list of topics to query, ''list(str)'' - :param start_stamp: stamp to start at, ''rospy.Time'' - :param end_stamp: stamp to end at, ''rospy,Time'' - :returns: entries the bag file, ''msg'' - """ - with self._bag_lock: - from rosbag import bag # for _mergesort - bag_entries = [] - for b in self._bags: - bag_start_time = bag_helper.get_start_stamp(b) - if bag_start_time is not None and bag_start_time > end_stamp: - continue - - bag_end_time = bag_helper.get_end_stamp(b) - if bag_end_time is not None and bag_end_time < start_stamp: - continue - - connections = list(b._get_connections(topics)) - bag_entries.append(b._get_entries(connections, start_stamp, end_stamp)) - - for entry, _ in bag._mergesort(bag_entries, key=lambda entry: entry.time): - yield entry - - def get_entries_with_bags(self, topic, start_stamp, end_stamp): - """ - generator function for bag entries - :param topics: list of topics to query, ''list(str)'' - :param start_stamp: stamp to start at, ''rospy.Time'' - :param end_stamp: stamp to end at, ''rospy,Time'' - :returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - from rosbag import bag # for _mergesort - - bag_entries = [] - bag_by_iter = {} - for b in self._bags: - bag_start_time = bag_helper.get_start_stamp(b) - if bag_start_time is not None and bag_start_time > end_stamp: - continue - - bag_end_time = bag_helper.get_end_stamp(b) - if bag_end_time is not None and bag_end_time < start_stamp: - continue - - connections = list(b._get_connections(topic)) - it = iter(b._get_entries(connections, start_stamp, end_stamp)) - bag_by_iter[it] = b - bag_entries.append(it) - - for entry, it in bag._mergesort(bag_entries, key=lambda entry: entry.time): - yield bag_by_iter[it], entry - - def get_entry(self, t, topic): - """ - Access a bag entry - :param t: time, ''rospy.Time'' - :param topic: the topic to be accessed, ''str'' - :return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - entry_bag, entry = None, None - for bag in self._bags: - bag_entry = bag._get_entry(t, bag._get_connections(topic)) - if bag_entry and (not entry or bag_entry.time > entry.time): - entry_bag, entry = bag, bag_entry - - return entry_bag, entry - - def get_entry_before(self, t): - """ - Access a bag entry - :param t: time, ''rospy.Time'' - :return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - entry_bag, entry = None, None - for bag in self._bags: - bag_entry = bag._get_entry(t-rospy.Duration(0,1), bag._get_connections()) - if bag_entry and (not entry or bag_entry.time < entry.time): - entry_bag, entry = bag, bag_entry - - return entry_bag, entry - - def get_entry_after(self, t): - """ - Access a bag entry - :param t: time, ''rospy.Time'' - :return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - entry_bag, entry = None, None - for bag in self._bags: - bag_entry = bag._get_entry_after(t, bag._get_connections()) - if bag_entry and (not entry or bag_entry.time < entry.time): - entry_bag, entry = bag, bag_entry - - return entry_bag, entry - - def get_next_message_time(self): - """ - :return: time of the next message after the current playhead position,''rospy.Time'' - """ - if self._timeline_frame.playhead is None: - return None - - _, entry = self.get_entry_after(self._timeline_frame.playhead) - if entry is None: - return self._timeline_frame._start_stamp - - return entry.time - - def get_previous_message_time(self): - """ - :return: time of the next message before the current playhead position,''rospy.Time'' - """ - if self._timeline_frame.playhead is None: - return None - - _, entry = self.get_entry_before(self._timeline_frame.playhead) - if entry is None: - return self._timeline_frame._end_stamp - - return entry.time - - def resume(self): - if (self._player): - self._player.resume() - - ### Copy messages to... - - def start_background_task(self, background_task): - """ - Verify that a background task is not currently running before starting a new one - :param background_task: name of the background task, ''str'' - """ - if self.background_task is not None: - QMessageBox(QMessageBox.Warning, 'Exclamation', 'Background operation already running:\n\n%s' % self.background_task, QMessageBox.Ok).exec_() - return False - - self.background_task = background_task - self.background_task_cancel = False - return True - - def stop_background_task(self): - self.background_task = None - - def copy_region_to_bag(self, filename): - if len(self._bags) > 0: - self._export_region(filename, self._timeline_frame.topics, self._timeline_frame.play_region[0], self._timeline_frame.play_region[1]) - - def _export_region(self, path, topics, start_stamp, end_stamp): - """ - Starts a thread to save the current selection to a new bag file - :param path: filesystem path to write to, ''str'' - :param topics: topics to write to the file, ''list(str)'' - :param start_stamp: start of area to save, ''rospy.Time'' - :param end_stamp: end of area to save, ''rospy.Time'' - """ - if not self.start_background_task('Copying messages to "%s"' % path): - return - # TODO implement a status bar area with information on the current save status - bag_entries = list(self.get_entries_with_bags(topics, start_stamp, end_stamp)) - - if self.background_task_cancel: - return - - # Get the total number of messages to copy - total_messages = len(bag_entries) - - # If no messages, prompt the user and return - if total_messages == 0: - QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found', QMessageBox.Ok).exec_() - self.stop_background_task() - return - - # Open the path for writing - try: - export_bag = rosbag.Bag(path, 'w') - except Exception: - QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error opening bag file [%s] for writing' % path, QMessageBox.Ok).exec_() - self.stop_background_task() - return - - # Run copying in a background thread - self._export_thread = threading.Thread(target=self._run_export_region, args=(export_bag, topics, start_stamp, end_stamp, bag_entries)) - self._export_thread.start() - - def _run_export_region(self, export_bag, topics, start_stamp, end_stamp, bag_entries): - """ - Threaded function that saves the current selection to a new bag file - :param export_bag: bagfile to write to, ''rosbag.bag'' - :param topics: topics to write to the file, ''list(str)'' - :param start_stamp: start of area to save, ''rospy.Time'' - :param end_stamp: end of area to save, ''rospy.Time'' - """ - total_messages = len(bag_entries) - update_step = max(1, total_messages / 100) - message_num = 1 - progress = 0 - # Write out the messages - for bag, entry in bag_entries: - if self.background_task_cancel: - break - try: - topic, msg, t = self.read_message(bag, entry.position) - export_bag.write(topic, msg, t) - except Exception as ex: - qWarning('Error exporting message at position %s: %s' % (str(entry.position), str(ex))) - export_bag.close() - self.stop_background_task() - return - - if message_num % update_step == 0 or message_num == total_messages: - new_progress = int(100.0 * (float(message_num) / total_messages)) - if new_progress != progress: - progress = new_progress - if not self.background_task_cancel: - self.background_progress = progress - self.status_bar_changed_signal.emit() - - message_num += 1 - - # Close the bag - try: - self.background_progress = 0 - self.status_bar_changed_signal.emit() - export_bag.close() - except Exception as ex: - QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error closing bag file [%s]: %s' % (export_bag.filename, str(ex)), QMessageBox.Ok).exec_() - self.stop_background_task() - - def read_message(self, bag, position): - with self._bag_lock: - return bag._read_message(position) - - ### Mouse events - def on_mouse_down(self, event): - if event.buttons() == Qt.LeftButton: - self._timeline_frame.on_left_down(event) - elif event.buttons() == Qt.MidButton: - self._timeline_frame.on_middle_down(event) - elif event.buttons() == Qt.RightButton: - topic = self._timeline_frame.map_y_to_topic(event.y()) - TimelinePopupMenu(self, event, topic) - - def on_mouse_up(self, event): - self._timeline_frame.on_mouse_up(event) - - def on_mouse_move(self, event): - self._timeline_frame.on_mouse_move(event) - - def on_mousewheel(self, event): - self._timeline_frame.on_mousewheel(event) - - # Zooming - - def zoom_in(self): - self._timeline_frame.zoom_in() - - def zoom_out(self): - self._timeline_frame.zoom_out() - - def reset_zoom(self): - self._timeline_frame.reset_zoom() - - def translate_timeline_left(self): - self._timeline_frame.translate_timeline_left() - - def translate_timeline_right(self): - self._timeline_frame.translate_timeline_right() - - ### Publishing - def is_publishing(self, topic): - return self._player and self._player.is_publishing(topic) - - def start_publishing(self, topic): - if not self._player and not self._create_player(): - return False - - self._player.start_publishing(topic) - return True - - def stop_publishing(self, topic): - if not self._player: - return False - - self._player.stop_publishing(topic) - return True - - def _create_player(self): - if not self._player: - try: - self._player = Player(self) - if self._publish_clock: - self._player.start_clock_publishing() - except Exception as ex: - qWarning('Error starting player; aborting publish: %s' % str(ex)) - return False - - return True - - def set_publishing_state(self, start_publishing): - if start_publishing: - for topic in self._timeline_frame.topics: - if not self.start_publishing(topic): - break - else: - for topic in self._timeline_frame.topics: - self.stop_publishing(topic) - - # property: play_all - def _get_play_all(self): - return self._play_all - - def _set_play_all(self, play_all): - if play_all == self._play_all: - return - - self._play_all = not self._play_all - - self.last_frame = None - self.last_playhead = None - self.desired_playhead = None - - play_all = property(_get_play_all, _set_play_all) - - def toggle_play_all(self): - self.play_all = not self.play_all - - ### Playing - def on_idle(self): - self._step_playhead() - - def _step_playhead(self): - """ - moves the playhead to the next position based on the desired position - """ - # Reset when the playing mode switchs - if self._timeline_frame.playhead != self.last_playhead: - self.last_frame = None - self.last_playhead = None - self.desired_playhead = None - - if self._play_all: - self.step_next_message() - else: - self.step_fixed() - - def step_fixed(self): - """ - Moves the playhead a fixed distance into the future based on the current play speed - """ - if self.play_speed == 0.0 or not self._timeline_frame.playhead: - self.last_frame = None - self.last_playhead = None - return - - now = rospy.Time.from_sec(time.time()) - if self.last_frame: - # Get new playhead - if self.stick_to_end: - new_playhead = self.end_stamp - else: - new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec((now - self.last_frame).to_sec() * self.play_speed) - - start_stamp, end_stamp = self._timeline_frame.play_region - - if new_playhead > end_stamp: - if self.wrap: - if self.play_speed > 0.0: - new_playhead = start_stamp - else: - new_playhead = end_stamp - else: - new_playhead = end_stamp - - if self.play_speed > 0.0: - self.stick_to_end = True - - elif new_playhead < start_stamp: - if self.wrap: - if self.play_speed < 0.0: - new_playhead = end_stamp - else: - new_playhead = start_stamp - else: - new_playhead = start_stamp - - # Update the playhead - self._timeline_frame.playhead = new_playhead - - self.last_frame = now - self.last_playhead = self._timeline_frame.playhead - - def step_next_message(self): - """ - Move the playhead to the next message - """ - if self.play_speed <= 0.0 or not self._timeline_frame.playhead: - self.last_frame = None - self.last_playhead = None - return - - if self.last_frame: - if not self.desired_playhead: - self.desired_playhead = self._timeline_frame.playhead - else: - delta = rospy.Time.from_sec(time.time()) - self.last_frame - if delta > rospy.Duration.from_sec(0.1): - delta = rospy.Duration.from_sec(0.1) - self.desired_playhead += delta - - # Get the occurrence of the next message - next_message_time = self.get_next_message_time() - - if next_message_time < self.desired_playhead: - self._timeline_frame.playhead = next_message_time - else: - self._timeline_frame.playhead = self.desired_playhead - - self.last_frame = rospy.Time.from_sec(time.time()) - self.last_playhead = self._timeline_frame.playhead - - ### Recording - - def record_bag(self, filename, all=True, topics=[], regex=False, limit=0): - try: - self._recorder = Recorder(filename, bag_lock=self._bag_lock, all=all, topics=topics, regex=regex, limit=limit) - except Exception, ex: - qWarning('Error opening bag for recording [%s]: %s' % (filename, str(ex))) - return - - self._recorder.add_listener(self._message_recorded) - - self.add_bag(self._recorder.bag) - - self._recorder.start() - - self.wrap = False - self._timeline_frame._index_cache_thread.period = 0.1 - - self.update() - - def toggle_recording(self): - if self._recorder: - self._recorder.toggle_paused() - self.update() - - def _message_recorded(self, topic, msg, t): - if self._timeline_frame._start_stamp is None: - self._timeline_frame._start_stamp = t - self._timeline_frame._end_stamp = t - self._timeline_frame._playhead = t - elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp: - self._timeline_frame._end_stamp = t - - if not self._timeline_frame.topics or topic not in self._timeline_frame.topics: - self._timeline_frame.topics = self._get_topics() - self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype() - - self._playhead_positions_cvs[topic] = threading.Condition() - self._messages_cvs[topic] = threading.Condition() - self._message_loaders[topic] = MessageLoaderThread(self, topic) - - if self._timeline_frame._stamp_left is None: - self.reset_zoom() - - # Notify the index caching thread that it has work to do - with self._timeline_frame.index_cache_cv: - self._timeline_frame.invalidated_caches.add(topic) - self._timeline_frame.index_cache_cv.notify() - - if topic in self._listeners: - for listener in self._listeners[topic]: - try: - listener.timeline_changed() - except Exception, ex: - qWarning('Error calling timeline_changed on %s: %s' % (type(listener), str(ex))) - - ### Views / listeners - def add_view(self, topic, frame): - self._views.append(frame) - - def has_listeners(self, topic): - return topic in self._listeners - - def add_listener(self, topic, listener): - self._listeners.setdefault(topic, []).append(listener) - - self._message_listener_threads[(topic, listener)] = MessageListenerThread(self, topic, listener) - # Notify the message listeners - self._message_loaders[topic].reset() - with self._playhead_positions_cvs[topic]: - self._playhead_positions_cvs[topic].notify_all() - - self.update() - - def remove_listener(self, topic, listener): - topic_listeners = self._listeners.get(topic) - if topic_listeners is not None and listener in topic_listeners: - topic_listeners.remove(listener) - - if len(topic_listeners) == 0: - del self._listeners[topic] - - # Stop the message listener thread - if (topic, listener) in self._message_listener_threads: - self._message_listener_threads[(topic, listener)].stop() - del self._message_listener_threads[(topic, listener)] - self.update() - - ### Playhead - - # property: play_speed - def _get_play_speed(self): - if self._timeline_frame._paused: - return 0.0 - return self._play_speed - - def _set_play_speed(self, play_speed): - if play_speed == self._play_speed: - return - - if play_speed > 0.0: - self._play_speed = min(self._max_play_speed, max(self._min_play_speed, play_speed)) - elif play_speed < 0.0: - self._play_speed = max(-self._max_play_speed, min(-self._min_play_speed, play_speed)) - else: - self._play_speed = play_speed - - if self._play_speed < 1.0: - self.stick_to_end = False - - self.update() - play_speed = property(_get_play_speed, _set_play_speed) - - def toggle_play(self): - if self._play_speed != 0.0: - self.play_speed = 0.0 - else: - self.play_speed = 1.0 - - def navigate_play(self): - self.play_speed = 1.0 - self.last_frame = rospy.Time.from_sec(time.time()) - self.last_playhead = self._timeline_frame.playhead - self._play_timer.start() - - def navigate_stop(self): - self.play_speed = 0.0 - self._play_timer.stop() - - def navigate_previous(self): - self.navigate_stop() - self._timeline_frame.playhead = self.get_previous_message_time() - self.last_playhead = self._timeline_frame.playhead - - def navigate_next(self): - self.navigate_stop() - self._timeline_frame.playhead = self.get_next_message_time() - self.last_playhead = self._timeline_frame.playhead - - def navigate_rewind(self): - if self._play_speed < 0.0: - new_play_speed = self._play_speed * 2.0 - elif self._play_speed == 0.0: - new_play_speed = -1.0 - else: - new_play_speed = self._play_speed * 0.5 - - self.play_speed = new_play_speed - - def navigate_fastforward(self): - if self._play_speed > 0.0: - new_play_speed = self._play_speed * 2.0 - elif self._play_speed == 0.0: - new_play_speed = 2.0 - else: - new_play_speed = self._play_speed * 0.5 - - self.play_speed = new_play_speed - - def navigate_start(self): - self._timeline_frame.playhead = self._timeline_frame.play_region[0] - - def navigate_end(self): - self._timeline_frame.playhead = self._timeline_frame.play_region[1] diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py deleted file mode 100644 index 714abf1c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py +++ /dev/null @@ -1,354 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import time - -import rospy -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, qWarning, Signal -from python_qt_binding.QtGui import QFileDialog, QGraphicsView, QIcon, QWidget - -import rosbag -import bag_helper -from .bag_timeline import BagTimeline -from .topic_selection import TopicSelection - - -class BagGraphicsView(QGraphicsView): - def __init__(self, parent=None): - super(BagGraphicsView, self).__init__() - - -class BagWidget(QWidget): - """ - Widget for use with Bag class to display and replay bag files - Handles all widget callbacks and contains the instance of BagTimeline for storing visualizing bag data - """ - - set_status_text = Signal(str) - - def __init__(self, context, publish_clock): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(BagWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_bag'), 'resource', 'bag_widget.ui') - loadUi(ui_file, self, {'BagGraphicsView': BagGraphicsView}) - - self.setObjectName('BagWidget') - - self._timeline = BagTimeline(context, publish_clock) - self.graphics_view.setScene(self._timeline) - - self.graphics_view.resizeEvent = self._resizeEvent - self.graphics_view.setMouseTracking(True) - - self.play_icon = QIcon.fromTheme('media-playback-start') - self.pause_icon = QIcon.fromTheme('media-playback-pause') - self.play_button.setIcon(self.play_icon) - self.begin_button.setIcon(QIcon.fromTheme('media-skip-backward')) - self.end_button.setIcon(QIcon.fromTheme('media-skip-forward')) - self.slower_button.setIcon(QIcon.fromTheme('media-seek-backward')) - self.faster_button.setIcon(QIcon.fromTheme('media-seek-forward')) - self.previous_button.setIcon(QIcon.fromTheme('go-previous')) - self.next_button.setIcon(QIcon.fromTheme('go-next')) - self.zoom_in_button.setIcon(QIcon.fromTheme('zoom-in')) - self.zoom_out_button.setIcon(QIcon.fromTheme('zoom-out')) - self.zoom_all_button.setIcon(QIcon.fromTheme('zoom-original')) - self.thumbs_button.setIcon(QIcon.fromTheme('insert-image')) - self.record_button.setIcon(QIcon.fromTheme('media-record')) - self.load_button.setIcon(QIcon.fromTheme('document-open')) - self.save_button.setIcon(QIcon.fromTheme('document-save')) - - self.play_button.clicked[bool].connect(self._handle_play_clicked) - self.thumbs_button.clicked[bool].connect(self._handle_thumbs_clicked) - self.zoom_in_button.clicked[bool].connect(self._handle_zoom_in_clicked) - self.zoom_out_button.clicked[bool].connect(self._handle_zoom_out_clicked) - self.zoom_all_button.clicked[bool].connect(self._handle_zoom_all_clicked) - self.previous_button.clicked[bool].connect(self._handle_previous_clicked) - self.next_button.clicked[bool].connect(self._handle_next_clicked) - self.faster_button.clicked[bool].connect(self._handle_faster_clicked) - self.slower_button.clicked[bool].connect(self._handle_slower_clicked) - self.begin_button.clicked[bool].connect(self._handle_begin_clicked) - self.end_button.clicked[bool].connect(self._handle_end_clicked) - self.record_button.clicked[bool].connect(self._handle_record_clicked) - self.load_button.clicked[bool].connect(self._handle_load_clicked) - self.save_button.clicked[bool].connect(self._handle_save_clicked) - self.graphics_view.mousePressEvent = self._timeline.on_mouse_down - self.graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up - self.graphics_view.mouseMoveEvent = self._timeline.on_mouse_move - self.graphics_view.wheelEvent = self._timeline.on_mousewheel - self.closeEvent = self.handle_close - self.keyPressEvent = self.on_key_press - # TODO when the closeEvent is properly called by ROS_GUI implement that event instead of destroyed - self.destroyed.connect(self.handle_destroy) - - self.graphics_view.keyPressEvent = self.graphics_view_on_key_press - self.play_button.setEnabled(False) - self.thumbs_button.setEnabled(False) - self.zoom_in_button.setEnabled(False) - self.zoom_out_button.setEnabled(False) - self.zoom_all_button.setEnabled(False) - self.previous_button.setEnabled(False) - self.next_button.setEnabled(False) - self.faster_button.setEnabled(False) - self.slower_button.setEnabled(False) - self.begin_button.setEnabled(False) - self.end_button.setEnabled(False) - self.save_button.setEnabled(False) - - self._recording = False - - self._timeline.status_bar_changed_signal.connect(self._update_status_bar) - self.set_status_text.connect(self._set_status_text) - - def graphics_view_on_key_press(self, event): - key = event.key() - if key in (Qt.Key_Left, Qt.Key_Right, Qt.Key_Up, Qt.Key_Down, Qt.Key_PageUp, Qt.Key_PageDown): - # This causes the graphics view to ignore these keys so they can be caught by the bag_widget keyPressEvent - event.ignore() - else: - # Maintains functionality for all other keys QGraphicsView implements - QGraphicsView.keyPressEvent(self.graphics_view, event) - - # callbacks for ui events - def on_key_press(self, event): - key = event.key() - if key == Qt.Key_Space: - self._timeline.toggle_play() - elif key == Qt.Key_Home: - self._timeline.navigate_start() - elif key == Qt.Key_End: - self._handle_end_clicked() - elif key == Qt.Key_Plus or key == Qt.Key_Equal: - self._handle_faster_clicked() - elif key == Qt.Key_Minus: - self._handle_slower_clicked() - elif key == Qt.Key_Left: - self._timeline.translate_timeline_left() - elif key == Qt.Key_Right: - self._timeline.translate_timeline_right() - elif key == Qt.Key_Up or key == Qt.Key_PageUp: - self._handle_zoom_in_clicked() - elif key == Qt.Key_Down or key == Qt.Key_PageDown: - self._handle_zoom_out_clicked() - - def handle_destroy(self, args): - self._timeline.handle_close() - - def handle_close(self, event): - self.shutdown_all() - - event.accept() - - def _resizeEvent(self, event): - # TODO The -2 allows a buffer zone to make sure the scroll bars do not appear when not needed. On some systems (Lucid) this doesn't function properly - # need to look at a method to determine the maximum size of the scene that will maintain a proper no scrollbar fit in the view. - self.graphics_view.scene().setSceneRect(0, 0, self.graphics_view.width() - 2, max(self.graphics_view.height() - 2, self._timeline._timeline_frame._history_bottom)) - - def _handle_publish_clicked(self, checked): - self._timeline.set_publishing_state(checked) - - def _handle_play_clicked(self, checked): - if checked: - self.play_button.setIcon(self.pause_icon) - self._timeline.navigate_play() - else: - self.play_button.setIcon(self.play_icon) - self._timeline.navigate_stop() - - def _handle_next_clicked(self): - self._timeline.navigate_next() - self.play_button.setChecked(False) - self.play_button.setIcon(self.play_icon) - - def _handle_previous_clicked(self): - self._timeline.navigate_previous() - self.play_button.setChecked(False) - self.play_button.setIcon(self.play_icon) - - def _handle_faster_clicked(self): - self._timeline.navigate_fastforward() - self.play_button.setChecked(True) - self.play_button.setIcon(self.pause_icon) - - def _handle_slower_clicked(self): - self._timeline.navigate_rewind() - self.play_button.setChecked(True) - self.play_button.setIcon(self.pause_icon) - - def _handle_begin_clicked(self): - self._timeline.navigate_start() - - def _handle_end_clicked(self): - self._timeline.navigate_end() - - def _handle_thumbs_clicked(self, checked): - self._timeline._timeline_frame.toggle_renderers() - - def _handle_zoom_all_clicked(self): - self._timeline.reset_zoom() - - def _handle_zoom_out_clicked(self): - self._timeline.zoom_out() - - def _handle_zoom_in_clicked(self): - self._timeline.zoom_in() - - def _handle_record_clicked(self): - if self._recording: - self._timeline.toggle_recording() - return - - #TODO Implement limiting by regex and by number of messages per topic - self.topic_selection = TopicSelection() - self.topic_selection.recordSettingsSelected.connect(self._on_record_settings_selected) - - - def _on_record_settings_selected(self, all_topics, selected_topics): - # TODO verify master is still running - filename = QFileDialog.getSaveFileName(self, self.tr('Select prefix for new Bag File'), '.', self.tr('Bag files {.bag} (*.bag)')) - if filename[0] != '': - prefix = filename[0].strip() - - # Get filename to record to - record_filename = time.strftime('%Y-%m-%d-%H-%M-%S.bag', time.localtime(time.time())) - if prefix.endswith('.bag'): - prefix = prefix[:-len('.bag')] - if prefix: - record_filename = '%s_%s' % (prefix, record_filename) - - rospy.loginfo('Recording to %s.' % record_filename) - - self.load_button.setEnabled(False) - self._recording = True - self._timeline.record_bag(record_filename, all_topics, selected_topics) - - - def _handle_load_clicked(self): - filename = QFileDialog.getOpenFileName(self, self.tr('Load from File'), '.', self.tr('Bag files {.bag} (*.bag)')) - if filename[0] != '': - self.load_bag(filename[0]) - - def load_bag(self, filename): - qWarning("Loading %s" % filename) - - # QProgressBar can EITHER: show text or show a bouncing loading bar, - # but apparently the text is hidden when the bounding loading bar is - # shown - #self.progress_bar.setRange(0, 0) - self.set_status_text.emit("Loading %s" % filename) - #progress_format = self.progress_bar.format() - #progress_text_visible = self.progress_bar.isTextVisible() - #self.progress_bar.setFormat("Loading %s" % filename) - #self.progress_bar.setTextVisible(True) - - bag = rosbag.Bag(filename) - self.play_button.setEnabled(True) - self.thumbs_button.setEnabled(True) - self.zoom_in_button.setEnabled(True) - self.zoom_out_button.setEnabled(True) - self.zoom_all_button.setEnabled(True) - self.next_button.setEnabled(True) - self.previous_button.setEnabled(True) - self.faster_button.setEnabled(True) - self.slower_button.setEnabled(True) - self.begin_button.setEnabled(True) - self.end_button.setEnabled(True) - self.save_button.setEnabled(True) - self.record_button.setEnabled(False) - self._timeline.add_bag(bag) - qWarning("Done loading %s" % filename ) - # put the progress bar back the way it was - self.set_status_text.emit("") - #self.progress_bar.setFormat(progress_format) - #self.progress_bar.setTextVisible(progress_text_visible) # causes a segfault :( - #self.progress_bar.setRange(0, 100) - # self clear loading filename - - def _handle_save_clicked(self): - filename = QFileDialog.getSaveFileName(self, self.tr('Save selected region to file...'), '.', self.tr('Bag files {.bag} (*.bag)')) - if filename[0] != '': - self._timeline.copy_region_to_bag(filename[0]) - - def _set_status_text(self, text): - if text: - self.progress_bar.setFormat(text) - self.progress_bar.setTextVisible(True) - else: - self.progress_bar.setTextVisible(False) - - def _update_status_bar(self): - if self._timeline._timeline_frame.playhead is None or self._timeline._timeline_frame.start_stamp is None: - return - # TODO Figure out why this function is causing a "RuntimeError: wrapped C/C++ object of %S has been deleted" on close if the playhead is moving - try: - # Background Process Status - self.progress_bar.setValue(self._timeline.background_progress) - - # Raw timestamp - self.stamp_label.setText('%.3fs' % self._timeline._timeline_frame.playhead.to_sec()) - - # Human-readable time - self.date_label.setText(bag_helper.stamp_to_str(self._timeline._timeline_frame.playhead)) - - # Elapsed time (in seconds) - self.seconds_label.setText('%.3fs' % (self._timeline._timeline_frame.playhead - self._timeline._timeline_frame.start_stamp).to_sec()) - - # Play speed - spd = self._timeline.play_speed - if spd != 0.0: - if spd > 1.0: - spd_str = '>> %.0fx' % spd - elif spd == 1.0: - spd_str = '>' - elif spd > 0.0: - spd_str = '> 1/%.0fx' % (1.0 / spd) - elif spd > -1.0: - spd_str = '< 1/%.0fx' % (1.0 / -spd) - elif spd == 1.0: - spd_str = '<' - else: - spd_str = '<< %.0fx' % -spd - self.playspeed_label.setText(spd_str) - else: - self.playspeed_label.setText('') - except: - return - # Shutdown all members - - def shutdown_all(self): - self._timeline.handle_close() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py deleted file mode 100644 index 3c43de32..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py +++ /dev/null @@ -1,86 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import threading -import time - - -class IndexCacheThread(threading.Thread): - """ - Updates invalid caches. - One thread per timeline. - """ - def __init__(self, timeline): - threading.Thread.__init__(self) - self.timeline = timeline - self._stop_flag = False - self.setDaemon(True) - self.start() - - def run(self): - while not self._stop_flag: - with self.timeline.index_cache_cv: - # Wait until the cache is dirty - while len(self.timeline.invalidated_caches) == 0: - self.timeline.index_cache_cv.wait() - if self._stop_flag: - return - # Update the index for one topic - total_topics = len(self.timeline.topics) - update_step = max(1, total_topics / 100) - topic_num = 1 - progress = 0 - updated = False - for topic in self.timeline.topics: - if topic in self.timeline.invalidated_caches: - updated = (self.timeline._update_index_cache(topic) > 0) - if topic_num % update_step == 0 or topic_num == total_topics: - new_progress = int(100.0 * (float(topic_num) / total_topics)) - if new_progress != progress: - progress = new_progress - if not self._stop_flag: - self.timeline.scene().background_progress = progress - self.timeline.scene().status_bar_changed_signal.emit() - topic_num += 1 - - if updated: - self.timeline.scene().background_progress = 0 - self.timeline.scene().status_bar_changed_signal.emit() - self.timeline.scene().update() - # Give the GUI some time to update - time.sleep(1.0) - - def stop(self): - self._stop_flag = True - cv = self.timeline.index_cache_cv - with cv: - cv.notify() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py deleted file mode 100644 index 371a0b1e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py +++ /dev/null @@ -1,86 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import threading - -from python_qt_binding.QtCore import QCoreApplication, QEvent -from python_qt_binding.QtCore import qWarning - - -class ListenerEvent(QEvent): - def __init__(self, data): - super(ListenerEvent, self).__init__(1024) # userdefined event constant - self.data = data - - -class MessageListenerThread(threading.Thread): - """ - Waits for new messages loaded on the given topic, then calls the message listener. - One thread per listener, topic pair. - """ - def __init__(self, timeline, topic, listener): - threading.Thread.__init__(self) - - self.timeline = timeline - self.topic = topic - self.listener = listener - self.bag_msg_data = None - self._stop_flag = False - self.setDaemon(True) - self.start() - - def run(self): - """ - Thread body. loops and notifies the listener of new messages - """ - while not self._stop_flag: - # Wait for a new message - cv = self.timeline._messages_cvs[self.topic] - with cv: - while (self.topic not in self.timeline._messages) or (self.bag_msg_data == self.timeline._messages[self.topic]): - cv.wait() - if self._stop_flag: - return - bag_msg_data = self.timeline._messages[self.topic] - # View the message - self.bag_msg_data = bag_msg_data - try: - event = ListenerEvent(bag_msg_data) - QCoreApplication.postEvent(self.listener, event) - except Exception, ex: - qWarning('Error notifying listener %s: %s' % (type(self.listener), str(ex))) - - def stop(self): - self._stop_flag = True - cv = self.timeline._messages_cvs[self.topic] - with cv: - cv.notify_all() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py deleted file mode 100644 index 61e56ea9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py +++ /dev/null @@ -1,112 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import threading - - -class MessageLoaderThread(threading.Thread): - """ - Waits for a new playhead position on the given topic, then loads the message at that position and notifies the view threads. - - One thread per topic. Maintains a cache of recently loaded messages. - """ - def __init__(self, timeline, topic): - threading.Thread.__init__(self) - - self.timeline = timeline - self.topic = topic - - self.bag_playhead_position = None - - self._message_cache_capacity = 50 - self._message_cache = {} - self._message_cache_keys = [] - - self._stop_flag = False - - self.setDaemon(True) - self.start() - - def reset(self): - self.bag_playhead_position = None - - def run(self): - while not self._stop_flag: - # Wait for a new entry - cv = self.timeline._playhead_positions_cvs[self.topic] - with cv: - while (self.topic not in self.timeline._playhead_positions) or (self.bag_playhead_position == self.timeline._playhead_positions[self.topic]): - cv.wait() - if self._stop_flag: - return - bag, playhead_position = self.timeline._playhead_positions[self.topic] - - self.bag_playhead_position = (bag, playhead_position) - - # Don't bother loading the message if there are no listeners - if not self.timeline.has_listeners(self.topic): - continue - - # Load the message - if playhead_position is None: - msg_data = None - else: - msg_data = self._get_message(bag, playhead_position) - - # Inform the views - messages_cv = self.timeline._messages_cvs[self.topic] - with messages_cv: - self.timeline._messages[self.topic] = (bag, msg_data) - messages_cv.notify_all() # notify all views that a message is loaded - - def _get_message(self, bag, position): - key = '%s%s' % (bag.filename, str(position)) - if key in self._message_cache: - return self._message_cache[key] - - msg_data = self.timeline.read_message(bag, position) - - self._message_cache[key] = msg_data - self._message_cache_keys.append(key) - - if len(self._message_cache) > self._message_cache_capacity: - oldest_key = self._message_cache_keys[0] - del self._message_cache[oldest_key] - self._message_cache_keys.remove(oldest_key) - - return msg_data - - def stop(self): - self._stop_flag = True - cv = self.timeline._playhead_positions_cvs[self.topic] - with cv: - cv.notify_all() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py deleted file mode 100644 index 06e6d6d6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py +++ /dev/null @@ -1,150 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Player listens to messages from the timeline and publishes them to ROS. -""" - -import rospy -import rosgraph_msgs - -from python_qt_binding.QtCore import QObject - -CLOCK_TOPIC = "/clock" - -class Player(QObject): - """ - This object handles publishing messages as the playhead passes over their position - """ - def __init__(self, timeline): - super(Player, self).__init__() - self.timeline = timeline - - self._publishing = set() - self._publishers = {} - - self._publish_clock = False - self._last_clock = rosgraph_msgs.msg.Clock() - self._resume = False - - def resume(self): - self._resume = True - - def is_publishing(self, topic): - return topic in self._publishing - - def start_publishing(self, topic): - if topic in self._publishing: - return - self._publishing.add(topic) - self.timeline.add_listener(topic, self) - - def stop_publishing(self, topic): - if topic not in self._publishing: - return - self.timeline.remove_listener(topic, self) - - if topic in self._publishers: - self._publishers[topic].unregister() - del self._publishers[topic] - - self._publishing.remove(topic) - - def start_clock_publishing(self): - if CLOCK_TOPIC not in self._publishers: - # Activate clock publishing only if the publisher was created successful - self._publish_clock = self.create_publisher(CLOCK_TOPIC, rosgraph_msgs.msg.Clock()) - - def stop_clock_publishing(self): - self._publish_clock = False - if CLOCK_TOPIC in self._publishers: - self._publishers[CLOCK_TOPIC].unregister() - del self._publishers[CLOCK_TOPIC] - - def stop(self): - for topic in list(self._publishing): - self.stop_publishing(topic) - self.stop_clock_publishing() - - def create_publisher(self, topic, msg): - try: - try: - self._publishers[topic] = rospy.Publisher(topic, type(msg), queue_size=100) - except TypeError: - self._publishers[topic] = rospy.Publisher(topic, type(msg)) - return True - except Exception as ex: - # Any errors, stop listening/publishing to this topic - rospy.logerr('Error creating publisher on topic %s for type %s. \nError text: %s' % (topic, str(type(msg)), str(ex))) - if topic != CLOCK_TOPIC: - self.stop_publishing(topic) - return False - - def message_viewed(self, bag, msg_data): - """ - When a message is viewed publish it - :param bag: the bag the message is in, ''rosbag.bag'' - :param msg_data: tuple of the message data and topic info, ''(str, msg)'' - """ - # Don't publish unless the playhead is moving. - if self.timeline.play_speed <= 0.0: - return - - topic, msg, clock = msg_data - - # Create publisher if this is the first message on the topic - if topic not in self._publishers: - self.create_publisher(topic, msg) - - if self._publish_clock: - time_msg = rosgraph_msgs.msg.Clock() - time_msg.clock = clock - if self._resume or self._last_clock.clock < time_msg.clock: - self._resume = False - self._last_clock = time_msg - self._publishers[CLOCK_TOPIC].publish(time_msg) - self._publishers[topic].publish(msg) - - def message_cleared(self): - pass - - def event(self, event): - """ - This function will be called to process events posted by post_event - it will call message_cleared or message_viewed with the relevant data - """ - bag, msg_data = event.data - if msg_data: - self.message_viewed(bag, msg_data) - else: - self.message_cleared() - return True diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/__init__.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py deleted file mode 100644 index 15ed054e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py +++ /dev/null @@ -1,95 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from python_qt_binding.QtCore import QObject - - -class MessageView(QObject): - """ - A message details renderer. When registered with rqt_bag, a MessageView is called - whenever the timeline playhead moves. - """ - name = 'Untitled' - - def __init__(self, timeline, topic): - super(MessageView, self).__init__() - self.timeline = timeline - self.topic = topic - - def message_viewed(self, bag, msg_details): - """ - View the message. - - @param bag: the bag file the message is contained in - @type bag: rosbag.Bag - @param msg_details: the details of the message to be viewed - @type msg_details: tuple (topic, msg, time) - @param topic: the message topic - @type topic: str - @param msg: the message - @param t: the message timestamp - @type t: rospy.Time - """ - pass - - def message_cleared(self): - """ - Clear the currently viewed message (if any). - """ - pass - - def timeline_changed(self): - """ - Called when the messages in a timeline change, e.g. if a new message is recorded, or - a bag file is added - """ - pass - - def close(self): - """ - Close the message view, releasing any resources. - """ - pass - -# NOTE: event function should not be changed in subclasses - def event(self, event): - """ - This function will be called to process events posted by post_event - it will call message_cleared or message_viewed with the relevant data - """ - bag, msg_data = event.data - if msg_data: - self.message_viewed(bag, msg_data) - else: - self.message_cleared() - return True diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py deleted file mode 100644 index d1a06183..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -class Plugin(object): - - """ - Interface for rqt_bag plugins. - User-defined plugins may either subclass `rqt_bag.plugin.Plugin` or according to duck typing implement only the needed methods. - """ - - def __init__(self): - pass - - def get_view_class(self): - """Return a class which is a child of rqt_bag.plugin.topic_message_view.TopicMessageView.""" - raise NotImplementedError() - - def get_renderer_class(self): - """ - Return a class which is a child of rqt_bag.plugin.timeline_renderer.TimelineRenderer. - To omit the renderer component simply return None. - """ - return None - - def get_message_types(self): - """ - Return alist of message types which this plugin operates on. - To allow your plugin to be run on all message types return ['*']. - """ - return [] diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py deleted file mode 100644 index 210b0fa0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py +++ /dev/null @@ -1,231 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -""" -Defines a raw view: a TopicMessageView that displays the message contents in a tree. -""" -import rospy -import codecs -import math - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QApplication, QAbstractItemView, QSizePolicy, QTreeWidget, QTreeWidgetItem -from .topic_message_view import TopicMessageView - - -class RawView(TopicMessageView): - name = 'Raw' - """ - Plugin to view a message in a treeview window - The message is loaded into a custum treewidget - """ - def __init__(self, timeline, parent, topic): - """ - :param timeline: timeline data object, ''BagTimeline'' - :param parent: widget that will be added to the ros_gui context, ''QWidget'' - """ - super(RawView, self).__init__(timeline, parent, topic) - self.message_tree = MessageTree(parent) - parent.layout().addWidget(self.message_tree) # This will automatically resize the message_tree to the windowsize - - def message_viewed(self, bag, msg_details): - super(RawView, self).message_viewed(bag, msg_details) - _, msg, t = msg_details # topic, msg, t = msg_details - if t is None: - self.message_cleared() - else: - self.message_tree.set_message(msg) - - def message_cleared(self): - TopicMessageView.message_cleared(self) - self.message_tree.set_message(None) - - -class MessageTree(QTreeWidget): - def __init__(self, parent): - super(MessageTree, self).__init__(parent) - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.setHeaderHidden(True) - self.setSelectionMode(QAbstractItemView.ExtendedSelection) - self._msg = None - - self._expanded_paths = None - self.keyPressEvent = self.on_key_press - - @property - def msg(self): - return self._msg - - def set_message(self, msg): - """ - Clears the tree view and displays the new message - :param msg: message object to display in the treeview, ''msg'' - """ - # Remember whether items were expanded or not before deleting - if self._msg: - for item in self.get_all_items(): - path = self.get_item_path(item) - if item.isExpanded(): - self._expanded_paths.add(path) - elif path in self._expanded_paths: - self._expanded_paths.remove(path) - self.clear() - if msg: - # Populate the tree - self._add_msg_object(None, '', '', msg, msg._type) - - if self._expanded_paths is None: - self._expanded_paths = set() - else: - # Expand those that were previously expanded, and collapse any paths that we've seen for the first time - for item in self.get_all_items(): - path = self.get_item_path(item) - if path in self._expanded_paths: - item.setExpanded(True) - else: - item.setExpanded(False) - self._msg = msg - self.update() - - # Keyboard handler - def on_key_press(self, event): - key, ctrl = event.key(), event.modifiers() & Qt.ControlModifier - if ctrl: - if key == ord('C') or key == ord('c'): - # Ctrl-C: copy text from selected items to clipboard - self._copy_text_to_clipboard() - event.accept() - elif key == ord('A') or key == ord('a'): - # Ctrl-A: select all - self._select_all() - - def _select_all(self): - for i in self.get_all_items(): - if not i.isSelected(): - i.setSelected(True) - i.setExpanded(True) - - def _copy_text_to_clipboard(self): - # Get tab indented text for all selected items - def get_distance(item, ancestor, distance=0): - parent = item.parent() - if parent == None: - return distance - else: - return get_distance(parent, ancestor, distance + 1) - text = '' - for i in self.get_all_items(): - if i in self.selectedItems(): - text += ('\t' * (get_distance(i, None))) + i.text(0) + '\n' - # Copy the text to the clipboard - clipboard = QApplication.clipboard() - clipboard.setText(text) - - def get_item_path(self, item): - return item.data(0, Qt.UserRole)[0].replace(' ', '') # remove spaces that may get introduced in indexing, e.g. [ 3] is [3] - - def get_all_items(self): - items = [] - try: - root = self.invisibleRootItem() - self.traverse(root, items.append) - except Exception: - # TODO: very large messages can cause a stack overflow due to recursion - pass - return items - - def traverse(self, root, function): - for i in range(root.childCount()): - child = root.child(i) - function(child) - self.traverse(child, function) - - def _add_msg_object(self, parent, path, name, obj, obj_type): - label = name - - if hasattr(obj, '__slots__'): - subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__] - elif type(obj) in [list, tuple]: - len_obj = len(obj) - if len_obj == 0: - subobjs = [] - else: - w = int(math.ceil(math.log10(len_obj))) - subobjs = [('[%*d]' % (w, i), subobj) for (i, subobj) in enumerate(obj)] - else: - subobjs = [] - - if type(obj) in [int, long, float]: - if type(obj) == float: - obj_repr = '%.6f' % obj - else: - obj_repr = str(obj) - - if obj_repr[0] == '-': - label += ': %s' % obj_repr - else: - label += ': %s' % obj_repr - - elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]: - # Ignore any binary data - obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0] - - # Truncate long representations - if len(obj_repr) >= 50: - obj_repr = obj_repr[:50] + '...' - - label += ': ' + obj_repr - item = QTreeWidgetItem([label]) - if name == '': - pass - elif path.find('.') == -1 and path.find('[') == -1: - self.addTopLevelItem(item) - else: - parent.addChild(item) - item.setData(0, Qt.UserRole, (path, obj_type)) - - for subobj_name, subobj in subobjs: - if subobj is None: - continue - - if path == '': - subpath = subobj_name # root field - elif subobj_name.startswith('['): - subpath = '%s%s' % (path, subobj_name) # list, dict, or tuple - else: - subpath = '%s.%s' % (path, subobj_name) # attribute (prefix with '.') - - if hasattr(subobj, '_type'): - subobj_type = subobj._type - else: - subobj_type = type(subobj).__name__ - - self._add_msg_object(item, subpath, subobj_name, subobj, subobj_type) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py deleted file mode 100644 index b6cd91e3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py +++ /dev/null @@ -1,79 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QObject - - -class TimelineRenderer(QObject): - """ - A custom renderer for interval of time of a topic on the timeline. - - @param msg_combine_px: don't draw discrete messages if they're less than this many pixels separated [default: 1.5] - @type msg_combine_px: float - """ - def __init__(self, timeline, msg_combine_px=1.5): - self.timeline = timeline - self.msg_combine_px = msg_combine_px - - def get_segment_height(self, topic): - """ - Get the height of the topic segment on the timeline. - - @param topic: topic name to draw - @type topic: str - @return: height in pixels of the topic segment. If none, the timeline default is used. - @rtype: int or None - """ - return None - - def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height): - """ - Draw the timeline segment. - - @param painter: QPainter context to render into - @param topic: topic name - @param stamp_start: start of the interval on the timeline - @param stamp_end: start of the interval on the timeline - @param x: x coordinate of the timeline interval - @param y: y coordinate of the timeline interval - @param width: width in pixels of the timeline interval - @param height: height in pixels of the timeline interval - @return: whether the interval was renderered - @rtype: bool - """ - return False - - def close(self): - """ - Close the renderer, releasing any resources. - """ - pass diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py deleted file mode 100644 index 4fd62cf2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py +++ /dev/null @@ -1,103 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -from .message_view import MessageView - -from python_qt_binding.QtGui import QAction, QIcon, QToolBar - - -class TopicMessageView(MessageView): - """ - A message view with a toolbar for navigating messages in a single topic. - """ - def __init__(self, timeline, parent, topic): - MessageView.__init__(self, timeline, topic) - - self._parent = parent - self._stamp = None - self._name = parent.objectName() - - self.toolbar = QToolBar() - self._first_action = QAction(QIcon.fromTheme('go-first'), '', self.toolbar) - self._first_action.triggered.connect(self.navigate_first) - self.toolbar.addAction(self._first_action) - self._prev_action = QAction(QIcon.fromTheme('go-previous'), '', self.toolbar) - self._prev_action.triggered.connect(self.navigate_previous) - self.toolbar.addAction(self._prev_action) - self._next_action = QAction(QIcon.fromTheme('go-next'), '', self.toolbar) - self._next_action.triggered.connect(self.navigate_next) - self.toolbar.addAction(self._next_action) - self._last_action = QAction(QIcon.fromTheme('go-last'), '', self.toolbar) - self._last_action.triggered.connect(self.navigate_last) - self.toolbar.addAction(self._last_action) - parent.layout().addWidget(self.toolbar) - - @property - def parent(self): - return self._parent - - @property - def stamp(self): - return self._stamp - - # MessageView implementation - - def message_viewed(self, bag, msg_details): - _, _, self._stamp = msg_details[:3] - - # Events - def navigate_first(self): - for entry in self.timeline.get_entries([self.topic], *self.timeline._timeline_frame.play_region): - self.timeline._timeline_frame.playhead = entry.time - break - - def navigate_previous(self): - last_entry = None - for entry in self.timeline.get_entries([self.topic], self.timeline._timeline_frame.start_stamp, self.timeline._timeline_frame.playhead): - if entry.time < self.timeline._timeline_frame.playhead: - last_entry = entry - - if last_entry: - self.timeline._timeline_frame.playhead = last_entry.time - - def navigate_next(self): - for entry in self.timeline.get_entries([self.topic], self.timeline._timeline_frame.playhead, self.timeline._timeline_frame.end_stamp): - if entry.time > self.timeline._timeline_frame.playhead: - self.timeline._timeline_frame.playhead = entry.time - break - - def navigate_last(self): - last_entry = None - for entry in self.timeline.get_entries([self.topic], *self.timeline._timeline_frame.play_region): - last_entry = entry - - if last_entry: - self.timeline._timeline_frame.playhead = last_entry.time diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py deleted file mode 100644 index 213db578..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py +++ /dev/null @@ -1,247 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Recorder subscribes to ROS messages and writes them to a bag file. -""" - -import Queue -import re -import threading -import time - -import rosbag -import rosgraph -import roslib -import rospy - -import sys - - -class Recorder(object): - def __init__(self, filename, bag_lock=None, all=True, topics=[], regex=False, limit=0, master_check_interval=1.0): - """ - Subscribe to ROS messages and record them to a bag file. - - @param filename: filename of bag to write to - @type filename: str - @param all: all topics are to be recorded [default: True] - @type all: bool - @param topics: topics (or regexes if regex is True) to record [default: empty list] - @type topics: list of str - @param regex: topics should be considered as regular expressions [default: False] - @type regex: bool - @param limit: record only this number of messages on each topic (if non-positive, then unlimited) [default: 0] - @type limit: int - @param master_check_interval: period (in seconds) to check master for new topic publications [default: 1] - @type master_check_interval: float - """ - self._all = all - self._topics = topics - self._regex = regex - self._limit = limit - self._master_check_interval = master_check_interval - - self._bag = rosbag.Bag(filename, 'w') - self._bag_lock = bag_lock if bag_lock else threading.Lock() - self._listeners = [] - self._subscriber_helpers = {} - self._limited_topics = set() - self._failed_topics = set() - self._last_update = time.time() - self._write_queue = Queue.Queue() - self._paused = False - self._stop_condition = threading.Condition() - self._stop_flag = False - - # Compile regular expressions - if self._regex: - self._regexes = [re.compile(t) for t in self._topics] - else: - self._regexes = None - - self._message_count = {} # topic -> int (track number of messages recorded on each topic) - - self._master_check_thread = threading.Thread(target=self._run_master_check) - self._write_thread = threading.Thread(target=self._run_write) - - @property - def bag(self): - return self._bag - - def add_listener(self, listener): - """ - Add a listener which gets called whenever a message is recorded. - @param listener: function to call - @type listener: function taking (topic, message, time) - """ - self._listeners.append(listener) - - def start(self): - """ - Start subscribing and recording messages to bag. - """ - self._master_check_thread.start() - self._write_thread.start() - - @property - def paused(self): - return self._paused - - def pause(self): - self._paused = True - - def unpause(self): - self._paused = False - - def toggle_paused(self): - self._paused = not self._paused - - def stop(self): - """ - Stop recording. - """ - with self._stop_condition: - self._stop_flag = True - self._stop_condition.notify_all() - - self._write_queue.put(self) - - ## Implementation - - def _run_master_check(self): - master = rosgraph.Master('rqt_bag_recorder') - - try: - while not self._stop_flag: - # Check for new topics - for topic, datatype in master.getPublishedTopics(''): - # Check if: - # the topic is already subscribed to, or - # we've failed to subscribe to it already, or - # we've already reached the message limit, or - # we don't want to subscribe - if topic in self._subscriber_helpers or topic in self._failed_topics or topic in self._limited_topics or not self._should_subscribe_to(topic): - continue - - try: - pytype = roslib.message.get_message_class(datatype) - - self._message_count[topic] = 0 - - self._subscriber_helpers[topic] = _SubscriberHelper(self, topic, pytype) - except Exception, ex: - print >> sys.stderr, 'Error subscribing to %s (ignoring): %s' % (topic, str(ex)) - self._failed_topics.add(topic) - - # Wait a while - self._stop_condition.acquire() - self._stop_condition.wait(self._master_check_interval) - - except Exception, ex: - print >> sys.stderr, 'Error recording to bag: %s' % str(ex) - - # Unsubscribe from all topics - for topic in list(self._subscriber_helpers.keys()): - self._unsubscribe(topic) - - # Close the bag file so that the index gets written - try: - self._bag.close() - except Exception, ex: - print >> sys.stderr, 'Error closing bag [%s]: %s' % (self._bag.filename, str(ex)) - - def _should_subscribe_to(self, topic): - if self._all: - return True - - if not self._regex: - return topic in self._topics - - for regex in self._regexes: - if regex.match(topic): - return True - - return False - - def _unsubscribe(self, topic): - try: - self._subscriber_helpers[topic].subscriber.unregister() - except Exception: - return - - del self._subscriber_helpers[topic] - - def _record(self, topic, m): - if self._paused: - return - - if self._limit and self._message_count[topic] >= self._limit: - self._limited_topics.add(topic) - self._unsubscribe(topic) - return - - self._write_queue.put((topic, m, rospy.get_rostime())) - self._message_count[topic] += 1 - - def _run_write(self): - try: - while not self._stop_flag: - # Wait for a message - item = self._write_queue.get() - - if item == self: - continue - - topic, m, t = item - - # Write to the bag - with self._bag_lock: - self._bag.write(topic, m, t) - - # Notify listeners that a message has been recorded - for listener in self._listeners: - listener(topic, m, t) - - except Exception, ex: - print >> sys.stderr, 'Error write to bag: %s' % str(ex) - - -class _SubscriberHelper(object): - def __init__(self, recorder, topic, pytype): - self.recorder = recorder - self.topic = topic - - self.subscriber = rospy.Subscriber(self.topic, pytype, self.callback) - - def callback(self, m): - self.recorder._record(self.topic, m) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py deleted file mode 100644 index 169afec5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py +++ /dev/null @@ -1,177 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -import bisect -import Queue -import threading -import time - - -class TimelineCache(threading.Thread): - """ - Caches items for timeline renderers - """ - def __init__(self, loader, listener=None, max_cache_size=100): - threading.Thread.__init__(self) - - self.loader = loader - self.listener = listener - self.stop_flag = False - self.lock = threading.RLock() - self.items = {} # topic -> [(timestamp, items), ...] - self.last_accessed = {} # topic -> [(access time, timestamp), ...] - self.item_access = {} # topic -> timestamp -> access time - self.max_cache_size = max_cache_size # max number of items to cache (per topic) - self.queue = Queue.Queue() - self.setDaemon(True) - self.start() - - def run(self): - while not self.stop_flag: - # Get next item to load - entry = self.queue.get() - # self used to signal a change in stop_flag - if entry == self: - continue - # Check we haven't already cached it - topic, stamp, time_threshold, item_details = entry - - if not self.get_item(topic, stamp, time_threshold): - # Load the item - msg_stamp, item = self.loader(topic, stamp, item_details) - if item: - # Store in the cache - self.cache_item(topic, msg_stamp, item) - - if self.listener: - self.listener(topic, msg_stamp, item) -# else: -# try: -# qWarning('Failed to load:%s' % entry) -# except: -# qWarning('Failed to load cache item') - self.queue.task_done() - - def enqueue(self, entry): - self.queue.put(entry) - - def cache_item(self, topic, t, item): - with self.lock: - if topic not in self.items: - self.items[topic] = [] - topic_cache = self.items[topic] - - cache_entry = (t.to_sec(), item) - cache_index = bisect.bisect_right(topic_cache, cache_entry) - topic_cache.insert(cache_index, cache_entry) - - self._update_last_accessed(topic, t.to_sec()) - - self._limit_cache() - - def get_item(self, topic, stamp, time_threshold): - with self.lock: - # Attempt to get a item from the cache that's within time_threshold secs from stamp - topic_cache = self.items.get(topic) - if topic_cache: - cache_index = max(0, bisect.bisect_right(topic_cache, (stamp, None)) - 1) - - if cache_index <= len(topic_cache) - 1: - # Get cache entry before (or at) timestamp, and entry after - (cache_before_stamp, cache_before_item) = topic_cache[cache_index] - if cache_index < len(topic_cache) - 1: - cache_after_stamp, cache_after_item = topic_cache[cache_index + 1] - else: - cache_after_stamp = None - - # Find closest entry - cache_before_dist = abs(stamp - cache_before_stamp) - if cache_after_stamp: - cache_after_dist = abs(cache_after_stamp - stamp) - - if cache_after_stamp and cache_after_dist < cache_before_dist: - cache_dist, cache_stamp, cache_item = cache_after_dist, cache_after_stamp, cache_after_item - else: - cache_dist, cache_stamp, cache_item = cache_before_dist, cache_before_stamp, cache_before_item - - # Check entry is close enough - if cache_dist <= time_threshold: - self._update_last_accessed(topic, cache_stamp) - return cache_item - return None - - def _update_last_accessed(self, topic, stamp): - """ - Maintains a sorted list of cache accesses by timestamp for each topic. - """ - with self.lock: - access_time = time.time() - - if topic not in self.last_accessed: - self.last_accessed[topic] = [(access_time, stamp)] - self.item_access[topic] = {stamp: access_time} - return - - topic_last_accessed = self.last_accessed[topic] - topic_item_access = self.item_access[topic] - - if stamp in topic_item_access: - last_access = topic_item_access[stamp] - - index = bisect.bisect_left(topic_last_accessed, (last_access, None)) - assert(topic_last_accessed[index][1] == stamp) - - del topic_last_accessed[index] - - topic_last_accessed.append((access_time, stamp)) - topic_item_access[stamp] = access_time - - def _limit_cache(self): - """ - Removes LRU's from cache until size of each topic's cache is <= max_cache_size. - """ - with self.lock: - for topic, topic_cache in self.items.items(): - while len(topic_cache) > self.max_cache_size: - lru_stamp = self.last_accessed[topic][0][1] - - cache_index = bisect.bisect_left(topic_cache, (lru_stamp, None)) - assert(topic_cache[cache_index][0] == lru_stamp) - - del topic_cache[cache_index] - del self.last_accessed[topic][0] - del self.item_access[topic][lru_stamp] - - def stop(self): - self.stop_flag = True - self.queue.put(self) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py deleted file mode 100644 index 9d090166..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py +++ /dev/null @@ -1,1175 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from python_qt_binding.QtCore import qDebug, QPointF, QRectF, Qt, qWarning, Signal -from python_qt_binding.QtGui import QBrush, QCursor, QColor, QFont, \ - QFontMetrics, QGraphicsItem, QPen, \ - QPolygonF -import rospy - -import bisect -import threading - -from .index_cache_thread import IndexCacheThread -from .plugins.raw_view import RawView - - -class _SelectionMode(object): - """ - SelectionMode states consolidated for readability - NONE = no region marked or started - LEFT_MARKED = one end of the region has been marked - MARKED = both ends of the region have been marked - SHIFTING = region is marked; currently dragging the region - MOVE_LEFT = region is marked; currently changing the left boundry of the selected region - MOVE_RIGHT = region is marked; currently changing the right boundry of the selected region - """ - NONE = 'none' - LEFT_MARKED = 'left marked' - MARKED = 'marked' - SHIFTING = 'shifting' - MOVE_LEFT = 'move left' - MOVE_RIGHT = 'move right' - - -class TimelineFrame(QGraphicsItem): - """ - TimelineFrame Draws the framing elements for the bag messages - (time delimiters, labels, topic names and backgrounds). - Also handles mouse callbacks since they interact closely with the drawn elements - """ - - def __init__(self, bag_timeline): - super(TimelineFrame, self).__init__() - self._bag_timeline = bag_timeline - self._clicked_pos = None - self._dragged_pos = None - - # Timeline boundries - self._start_stamp = None # earliest of all stamps - self._end_stamp = None # latest of all stamps - self._stamp_left = None # earliest currently visible timestamp on the timeline - self._stamp_right = None # latest currently visible timestamp on the timeline - self._history_top = 30 - self._history_left = 0 - self._history_width = 0 - self._history_bottom = 0 - self._history_bounds = {} - self._margin_left = 4 - self._margin_right = 20 - self._margin_bottom = 20 - self._history_top = 30 - - # Background Rendering - self._bag_end_color = QColor(0, 0, 0, 25) # color of background of timeline before first message and after last - self._history_background_color_alternate = QColor(179, 179, 179, 25) - self._history_background_color = QColor(204, 204, 204, 102) - - # Timeline Division Rendering - # Possible time intervals used between divisions - # 1ms, 5ms, 10ms, 50ms, 100ms, 500ms - # 1s, 5s, 15s, 30s - # 1m, 2m, 5m, 10m, 15m, 30m - # 1h, 2h, 3h, 6h, 12h - # 1d, 7d - self._sec_divisions = [0.001, 0.005, 0.01, 0.05, 0.1, 0.5, - 1, 5, 15, 30, - 1 * 60, 2 * 60, 5 * 60, 10 * 60, 15 * 60, 30 * 60, - 1 * 60 * 60, 2 * 60 * 60, 3 * 60 * 60, 6 * 60 * 60, 12 * 60 * 60, - 1 * 60 * 60 * 24, 7 * 60 * 60 * 24] - self._minor_spacing = 15 - self._major_spacing = 50 - self._major_divisions_label_indent = 3 # padding in px between line and label - self._major_division_pen = QPen(QBrush(Qt.black), 0, Qt.DashLine) - self._minor_division_pen = QPen(QBrush(QColor(153, 153, 153, 128)), 0, Qt.DashLine) - self._minor_division_tick_pen = QPen(QBrush(QColor(128, 128, 128, 128)), 0) - - # Topic Rendering - self.topics = [] - self._topics_by_datatype = {} - self._topic_font_height = None - self._topic_name_sizes = None - self._topic_name_spacing = 3 # minimum pixels between end of topic name and start of history - self._topic_font_size = 10.0 - self._topic_font = QFont("cairo") - self._topic_font.setPointSize(self._topic_font_size) - self._topic_font.setBold(False) - self._topic_vertical_padding = 4 - self._topic_name_max_percent = 25.0 # percentage of the horiz space that can be used for topic display - - # Time Rendering - self._time_tick_height = 5 - self._time_font_height = None - self._time_font_size = 10.0 - self._time_font = QFont("cairo") - self._time_font.setPointSize(self._time_font_size) - self._time_font.setBold(False) - - # Defaults - self._default_brush = QBrush(Qt.black, Qt.SolidPattern) - self._default_pen = QPen(Qt.black) - self._default_datatype_color = QColor(0, 0, 102, 204) - self._datatype_colors = { - 'sensor_msgs/CameraInfo': QColor(0, 0, 77, 204), - 'sensor_msgs/Image': QColor(0, 77, 77, 204), - 'sensor_msgs/LaserScan': QColor(153, 0, 0, 204), - 'pr2_msgs/LaserScannerSignal': QColor(153, 0, 0, 204), - 'pr2_mechanism_msgs/MechanismState': QColor(0, 153, 0, 204), - 'tf/tfMessage': QColor(0, 153, 0, 204), - } - self._default_msg_combine_px = 1.0 # minimum number of pixels allowed between two bag messages before they are combined - self._active_message_line_width = 3 - - # Selected Region Rendering - self._selected_region_color = QColor(0, 179, 0, 21) - self._selected_region_outline_top_color = QColor(0.0, 77, 0.0, 51) - self._selected_region_outline_ends_color = QColor(0.0, 77, 0.0, 102) - self._selecting_mode = _SelectionMode.NONE - self._selected_left = None - self._selected_right = None - self._selection_handle_width = 3.0 - - # Playhead Rendering - self._playhead = None # timestamp of the playhead - self._paused = False - self._playhead_pointer_size = (6, 6) - self._playhead_line_width = 1 - self._playhead_color = QColor(255, 0, 0, 191) - - # Zoom - self._zoom_sensitivity = 0.005 - self._min_zoom_speed = 0.5 - self._max_zoom_speed = 2.0 - self._min_zoom = 0.0001 # max zoom out (in px/s) - self._max_zoom = 50000.0 # max zoom in (in px/s) - - # Plugin management - self._viewer_types = {} - self._timeline_renderers = {} - self._rendered_topics = set() - self.load_plugins() - - # Bag indexer for rendering the default message views on the timeline - self.index_cache_cv = threading.Condition() - self.index_cache = {} - self.invalidated_caches = set() - self._index_cache_thread = IndexCacheThread(self) - - # TODO the API interface should exist entirely at the bag_timeline level. Add a "get_draw_parameters()" at the bag_timeline level to access these - # Properties, work in progress API for plugins: - - # property: playhead - def _get_playhead(self): - return self._playhead - - def _set_playhead(self, playhead): - """ - Sets the playhead to the new position, notifies the threads and updates the scene so it will redraw - :signal: emits status_bar_changed_signal if the playhead is successfully set - :param playhead: Time to set the playhead to, ''rospy.Time()'' - """ - with self.scene()._playhead_lock: - if playhead == self._playhead: - return - - self._playhead = playhead - if self._playhead != self._end_stamp: - self.scene().stick_to_end = False - - playhead_secs = playhead.to_sec() - if playhead_secs > self._stamp_right: - dstamp = playhead_secs - self._stamp_right + (self._stamp_right - self._stamp_left) * 0.75 - if dstamp > self._end_stamp.to_sec() - self._stamp_right: - dstamp = self._end_stamp.to_sec() - self._stamp_right - self.translate_timeline(dstamp) - - elif playhead_secs < self._stamp_left: - dstamp = self._stamp_left - playhead_secs + (self._stamp_right - self._stamp_left) * 0.75 - if dstamp > self._stamp_left - self._start_stamp.to_sec(): - dstamp = self._stamp_left - self._start_stamp.to_sec() - self.translate_timeline(-dstamp) - - # Update the playhead positions - for topic in self.topics: - bag, entry = self.scene().get_entry(self._playhead, topic) - if entry: - if topic in self.scene()._playhead_positions and self.scene()._playhead_positions[topic] == (bag, entry.position): - continue - new_playhead_position = (bag, entry.position) - else: - new_playhead_position = (None, None) - with self.scene()._playhead_positions_cvs[topic]: - self.scene()._playhead_positions[topic] = new_playhead_position - self.scene()._playhead_positions_cvs[topic].notify_all() # notify all message loaders that a new message needs to be loaded - self.scene().update() - self.scene().status_bar_changed_signal.emit() - - playhead = property(_get_playhead, _set_playhead) - - # TODO add more api variables here to allow plugin access - @property - def _history_right(self): - return self._history_left + self._history_width - - @property - def has_selected_region(self): - return self._selected_left is not None and self._selected_right is not None - - @property - def play_region(self): - if self.has_selected_region: - return (rospy.Time.from_sec(self._selected_left), rospy.Time.from_sec(self._selected_right)) - else: - return (self._start_stamp, self._end_stamp) - - def emit_play_region(self): - play_region = self.play_region - if(play_region[0] is not None and play_region[1] is not None): - self.scene().selected_region_changed.emit(*play_region) - - @property - def start_stamp(self): - return self._start_stamp - - @property - def end_stamp(self): - return self._end_stamp - - # QGraphicsItem implementation - def boundingRect(self): - return QRectF(0, 0, self._history_left + self._history_width + self._margin_right, self._history_bottom + self._margin_bottom) - - def paint(self, painter, option, widget): - if self._start_stamp is None: - return - - self._layout() - self._draw_topic_dividers(painter) - self._draw_selected_region(painter) - self._draw_time_divisions(painter) - self._draw_topic_histories(painter) - self._draw_bag_ends(painter) - self._draw_topic_names(painter) - self._draw_history_border(painter) - self._draw_playhead(painter) - # END QGraphicsItem implementation - - # Drawing Functions - - def _qfont_width(self, name): - return QFontMetrics(self._topic_font).width(name) - - def _trimmed_topic_name(self, topic_name): - """ - This function trims the topic name down to a reasonable percentage of the viewable scene area - """ - allowed_width = self._scene_width * (self._topic_name_max_percent / 100.0) - allowed_width = allowed_width - self._topic_name_spacing - self._margin_left - trimmed_return = topic_name - if allowed_width < self._qfont_width(topic_name): - # We need to trim the topic - trimmed = '' - split_name = topic_name.split('/') - split_name = filter(lambda a: a != '', split_name) - # Save important last element of topic name provided it is small - popped_last = False - if self._qfont_width(split_name[-1]) < .5 * allowed_width: - popped_last = True - last_item = split_name[-1] - split_name = split_name[:-1] - allowed_width = allowed_width - self._qfont_width(last_item) - # Shorten and add remaining items keeping lenths roughly equal - for item in split_name: - if self._qfont_width(item) > allowed_width / float(len(split_name)): - trimmed_item = item[:-3] + '..' - while self._qfont_width(trimmed_item) > allowed_width / float(len(split_name)): - if len(trimmed_item) >= 3: - trimmed_item = trimmed_item[:-3] + '..' - else: - break - trimmed = trimmed + '/' + trimmed_item - else: - trimmed = trimmed + '/' + item - if popped_last: - trimmed = trimmed + '/' + last_item - trimmed = trimmed[1:] - trimmed_return = trimmed - return trimmed_return - - def _layout(self): - """ - Recalculates the layout of the of the timeline to take into account any changes that have occured - """ - # Calculate history left and history width - self._scene_width = self.scene().views()[0].size().width() - - max_topic_name_width = -1 - for topic in self.topics: - topic_width = self._qfont_width(self._trimmed_topic_name(topic)) - if max_topic_name_width <= topic_width: - max_topic_name_width = topic_width - - # Calculate font height for each topic - self._topic_font_height = -1 - for topic in self.topics: - topic_height = QFontMetrics(self._topic_font).height() - if self._topic_font_height <= topic_height: - self._topic_font_height = topic_height - - # Update the timeline boundries - new_history_left = self._margin_left + max_topic_name_width + self._topic_name_spacing - new_history_width = self._scene_width - new_history_left - self._margin_right - self._history_left = new_history_left - self._history_width = new_history_width - - # Calculate the bounds for each topic - self._history_bounds = {} - y = self._history_top - for topic in self.topics: - datatype = self.scene().get_datatype(topic) - - topic_height = None - if topic in self._rendered_topics: - renderer = self._timeline_renderers.get(datatype) - if renderer: - topic_height = renderer.get_segment_height(topic) - if not topic_height: - topic_height = self._topic_font_height + self._topic_vertical_padding - - self._history_bounds[topic] = (self._history_left, y, self._history_width, topic_height) - - y += topic_height - -# new_history_bottom = max([y + h for (x, y, w, h) in self._history_bounds.values()]) - 1 - new_history_bottom = max([y + h for (_, y, _, h) in self._history_bounds.values()]) - 1 - if new_history_bottom != self._history_bottom: - self._history_bottom = new_history_bottom - - def _draw_topic_histories(self, painter): - """ - Draw all topic messages - :param painter: allows access to paint functions,''QPainter'' - """ - for topic in sorted(self._history_bounds.keys()): - self._draw_topic_history(painter, topic) - - def _draw_topic_history(self, painter, topic): - """ - Draw boxes corrisponding to message regions on the timeline. - :param painter: allows access to paint functions,''QPainter'' - :param topic: the topic for which message boxes should be drawn, ''str'' - """ - -# x, y, w, h = self._history_bounds[topic] - _, y, _, h = self._history_bounds[topic] - - msg_y = y + 2 - msg_height = h - 2 - - datatype = self.scene().get_datatype(topic) - - # Get the renderer and the message combine interval - renderer = None - msg_combine_interval = None - if topic in self._rendered_topics: - renderer = self._timeline_renderers.get(datatype) - if not renderer is None: - msg_combine_interval = self.map_dx_to_dstamp(renderer.msg_combine_px) - if msg_combine_interval is None: - msg_combine_interval = self.map_dx_to_dstamp(self._default_msg_combine_px) - - # Get the cache - if topic not in self.index_cache: - return - all_stamps = self.index_cache[topic] - -# start_index = bisect.bisect_left(all_stamps, self._stamp_left) - end_index = bisect.bisect_left(all_stamps, self._stamp_right) - # Set pen based on datatype - datatype_color = self._datatype_colors.get(datatype, self._default_datatype_color) - # Iterate through regions of connected messages - width_interval = self._history_width / (self._stamp_right - self._stamp_left) - - # Draw stamps - for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], self.map_dx_to_dstamp(self._default_msg_combine_px)): - if stamp_end < self._stamp_left: - continue - - region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval - if region_x_start < self._history_left: - region_x_start = self._history_left # Clip the region - region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval - region_width = max(1, region_x_end - region_x_start) - - painter.setBrush(QBrush(datatype_color)) - painter.setPen(QPen(datatype_color, 1)) - painter.drawRect(region_x_start, msg_y, region_width, msg_height) - - # Draw active message - if topic in self.scene()._listeners: - curpen = painter.pen() - oldwidth = curpen.width() - curpen.setWidth(self._active_message_line_width) - painter.setPen(curpen) - playhead_stamp = None - playhead_index = bisect.bisect_right(all_stamps, self.playhead.to_sec()) - 1 - if playhead_index >= 0: - playhead_stamp = all_stamps[playhead_index] - if playhead_stamp > self._stamp_left and playhead_stamp < self._stamp_right: - playhead_x = self._history_left + (all_stamps[playhead_index] - self._stamp_left) * width_interval - painter.drawLine(playhead_x, msg_y, playhead_x, msg_y + msg_height) - curpen.setWidth(oldwidth) - painter.setPen(curpen) - - # Custom renderer - if renderer: - # Iterate through regions of connected messages - for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], msg_combine_interval): - if stamp_end < self._stamp_left: - continue - - region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval - region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval - region_width = max(1, region_x_end - region_x_start) - renderer.draw_timeline_segment(painter, topic, stamp_start, stamp_end, region_x_start, msg_y, region_width, msg_height) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_bag_ends(self, painter): - """ - Draw markers to indicate the area the bag file represents within the current visible area. - :param painter: allows access to paint functions,''QPainter'' - """ - x_start, x_end = self.map_stamp_to_x(self._start_stamp.to_sec()), self.map_stamp_to_x(self._end_stamp.to_sec()) - painter.setBrush(QBrush(self._bag_end_color)) - painter.drawRect(self._history_left, self._history_top, x_start - self._history_left, self._history_bottom - self._history_top) - painter.drawRect(x_end, self._history_top, self._history_left + self._history_width - x_end, self._history_bottom - self._history_top) - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_topic_dividers(self, painter): - """ - Draws horizontal lines between each topic to visually separate the messages - :param painter: allows access to paint functions,''QPainter'' - """ - clip_left = self._history_left - clip_right = self._history_left + self._history_width - - row = 0 - for topic in self.topics: - (x, y, w, h) = self._history_bounds[topic] - - if row % 2 == 0: - painter.setPen(Qt.lightGray) - painter.setBrush(QBrush(self._history_background_color_alternate)) - else: - painter.setPen(Qt.lightGray) - painter.setBrush(QBrush(self._history_background_color)) - left = max(clip_left, x) - painter.drawRect(left, y, min(clip_right - left, w), h) - row += 1 - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_selected_region(self, painter): - """ - Draws a box around the selected region - :param painter: allows access to paint functions,''QPainter'' - """ - if self._selected_left is None: - return - - x_left = self.map_stamp_to_x(self._selected_left) - if self._selected_right is not None: - x_right = self.map_stamp_to_x(self._selected_right) - else: - x_right = self.map_stamp_to_x(self.playhead.to_sec()) - - left = x_left - top = self._history_top - self._playhead_pointer_size[1] - 5 - self._time_font_size - 4 - width = x_right - x_left - height = self._history_top - top - - painter.setPen(self._selected_region_color) - painter.setBrush(QBrush(self._selected_region_color)) - painter.drawRect(left, top, width, height) - - painter.setPen(self._selected_region_outline_ends_color) - painter.setBrush(Qt.NoBrush) - painter.drawLine(left, top, left, top + height) - painter.drawLine(left + width, top, left + width, top + height) - - painter.setPen(self._selected_region_outline_top_color) - painter.setBrush(Qt.NoBrush) - painter.drawLine(left, top, left + width, top) - - painter.setPen(self._selected_region_outline_top_color) - painter.drawLine(left, self._history_top, left, self._history_bottom) - painter.drawLine(left + width, self._history_top, left + width, self._history_bottom) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_playhead(self, painter): - """ - Draw a line and 2 triangles to denote the current position being viewed - :param painter: ,''QPainter'' - """ - px = self.map_stamp_to_x(self.playhead.to_sec()) - pw, ph = self._playhead_pointer_size - - # Line - painter.setPen(QPen(self._playhead_color)) - painter.setBrush(QBrush(self._playhead_color)) - painter.drawLine(px, self._history_top - 1, px, self._history_bottom + 2) - - # Upper triangle - py = self._history_top - ph - painter.drawPolygon(QPolygonF([QPointF(px, py + ph), QPointF(px + pw, py), QPointF(px - pw, py)])) - - # Lower triangle - py = self._history_bottom + 1 - painter.drawPolygon(QPolygonF([QPointF(px, py), QPointF(px + pw, py + ph), QPointF(px - pw, py + ph)])) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_history_border(self, painter): - """ - Draw a simple black rectangle frame around the timeline view area - :param painter: ,''QPainter'' - """ - bounds_width = min(self._history_width, self.scene().width()) - x, y, w, h = self._history_left, self._history_top, bounds_width, self._history_bottom - self._history_top - - painter.setBrush(Qt.NoBrush) - painter.setPen(Qt.black) - painter.drawRect(x, y, w, h) - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_topic_names(self, painter): - """ - Calculate positions of existing topic names and draw them on the left, one for each row - :param painter: ,''QPainter'' - """ - topics = self._history_bounds.keys() - coords = [(self._margin_left, y + (h / 2) + (self._topic_font_height / 2)) for (_, y, _, h) in self._history_bounds.values()] - - for text, coords in zip([t.lstrip('/') for t in topics], coords): - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - painter.setFont(self._topic_font) - painter.drawText(coords[0], coords[1], self._trimmed_topic_name(text)) - - def _draw_time_divisions(self, painter): - """ - Draw vertical grid-lines showing major and minor time divisions. - :param painter: allows access to paint functions,''QPainter'' - """ - x_per_sec = self.map_dstamp_to_dx(1.0) - major_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._major_spacing] - if len(major_divisions) == 0: - major_division = max(self._sec_divisions) - else: - major_division = min(major_divisions) - - minor_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._minor_spacing and major_division % s == 0] - if len(minor_divisions) > 0: - minor_division = min(minor_divisions) - else: - minor_division = None - - start_stamp = self._start_stamp.to_sec() - - major_stamps = list(self._get_stamps(start_stamp, major_division)) - self._draw_major_divisions(painter, major_stamps, start_stamp, major_division) - - if minor_division: - minor_stamps = [s for s in self._get_stamps(start_stamp, minor_division) if s not in major_stamps] - self._draw_minor_divisions(painter, minor_stamps, start_stamp, minor_division) - - def _draw_major_divisions(self, painter, stamps, start_stamp, division): - """ - Draw black hashed vertical grid-lines showing major time divisions. - :param painter: allows access to paint functions,''QPainter'' - """ - label_y = self._history_top - self._playhead_pointer_size[1] - 5 - for stamp in stamps: - x = self.map_stamp_to_x(stamp, False) - - label = self._get_label(division, stamp - start_stamp) - label_x = x + self._major_divisions_label_indent - if label_x + self._qfont_width(label) < self.scene().width(): - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - painter.setFont(self._time_font) - painter.drawText(label_x, label_y, label) - - painter.setPen(self._major_division_pen) - painter.drawLine(x, label_y - self._time_tick_height - self._time_font_size, x, self._history_bottom) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_minor_divisions(self, painter, stamps, start_stamp, division): - """ - Draw grey hashed vertical grid-lines showing minor time divisions. - :param painter: allows access to paint functions,''QPainter'' - """ - xs = [self.map_stamp_to_x(stamp) for stamp in stamps] - painter.setPen(self._minor_division_pen) - for x in xs: - painter.drawLine(x, self._history_top, x, self._history_bottom) - - painter.setPen(self._minor_division_tick_pen) - for x in xs: - painter.drawLine(x, self._history_top - self._time_tick_height, x, self._history_top) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - # Close function - - def handle_close(self): - for renderer in self._timeline_renderers.values(): - renderer.close() - self._index_cache_thread.stop() - - # Plugin interaction functions - - def get_viewer_types(self, datatype): - return [RawView] + self._viewer_types.get('*', []) + self._viewer_types.get(datatype, []) - - def load_plugins(self): - from rqt_gui.rospkg_plugin_provider import RospkgPluginProvider - self.plugin_provider = RospkgPluginProvider('rqt_bag', 'rqt_bag::Plugin') - - plugin_descriptors = self.plugin_provider.discover(None) - for plugin_descriptor in plugin_descriptors: - try: - plugin = self.plugin_provider.load(plugin_descriptor.plugin_id(), plugin_context=None) - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to load plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - continue - try: - view = plugin.get_view_class() - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get view from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - continue - - timeline_renderer = None - try: - timeline_renderer = plugin.get_renderer_class() - except AttributeError: - pass - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get renderer from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - - msg_types = [] - try: - msg_types = plugin.get_message_types() - except AttributeError: - pass - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get message types from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - finally: - if not msg_types: - qWarning('rqt_bag.TimelineFrame.load_plugins() plugin "%s" declares no message types:\n%s' % (plugin_descriptor.plugin_id(), e)) - - for msg_type in msg_types: - self._viewer_types.setdefault(msg_type, []).append(view) - if timeline_renderer: - self._timeline_renderers[msg_type] = timeline_renderer(self) - - qDebug('rqt_bag.TimelineFrame.load_plugins() loaded plugin "%s"' % plugin_descriptor.plugin_id()) - - # Timeline renderer interaction functions - - def get_renderers(self): - """ - :returns: a list of the currently loaded renderers for the plugins - """ - renderers = [] - - for topic in self.topics: - datatype = self.scene().get_datatype(topic) - renderer = self._timeline_renderers.get(datatype) - if renderer is not None: - renderers.append((topic, renderer)) - return renderers - - def is_renderer_active(self, topic): - return topic in self._rendered_topics - - def toggle_renderers(self): - idle_renderers = len(self._rendered_topics) < len(self.topics) - - self.set_renderers_active(idle_renderers) - - def set_renderers_active(self, active): - if active: - for topic in self.topics: - self._rendered_topics.add(topic) - else: - self._rendered_topics.clear() - self.scene().update() - - def set_renderer_active(self, topic, active): - if active: - if topic in self._rendered_topics: - return - self._rendered_topics.add(topic) - else: - if not topic in self._rendered_topics: - return - self._rendered_topics.remove(topic) - self.scene().update() - - # Index Caching functions - - def _update_index_cache(self, topic): - """ - Updates the cache of message timestamps for the given topic. - :return: number of messages added to the index cache - """ - if self._start_stamp is None or self._end_stamp is None: - return 0 - - if topic not in self.index_cache: - # Don't have any cache of messages in this topic - start_time = self._start_stamp - topic_cache = [] - self.index_cache[topic] = topic_cache - else: - topic_cache = self.index_cache[topic] - - # Check if the cache has been invalidated - if topic not in self.invalidated_caches: - return 0 - - if len(topic_cache) == 0: - start_time = self._start_stamp - else: - start_time = rospy.Time.from_sec(max(0.0, topic_cache[-1])) - - end_time = self._end_stamp - - topic_cache_len = len(topic_cache) - - for entry in self.scene().get_entries(topic, start_time, end_time): - topic_cache.append(entry.time.to_sec()) - - if topic in self.invalidated_caches: - self.invalidated_caches.remove(topic) - - return len(topic_cache) - topic_cache_len - - def _find_regions(self, stamps, max_interval): - """ - Group timestamps into regions connected by timestamps less than max_interval secs apart - :param start_stamp: a list of stamps, ''list'' - :param stamp_step: seconds between each division, ''int'' - """ - region_start, prev_stamp = None, None - for stamp in stamps: - if prev_stamp: - if stamp - prev_stamp > max_interval: - region_end = prev_stamp - yield (region_start, region_end) - region_start = stamp - else: - region_start = stamp - - prev_stamp = stamp - - if region_start and prev_stamp: - yield (region_start, prev_stamp) - - def _get_stamps(self, start_stamp, stamp_step): - """ - Generate visible stamps every stamp_step - :param start_stamp: beginning of timeline stamp, ''int'' - :param stamp_step: seconds between each division, ''int'' - """ - if start_stamp >= self._stamp_left: - stamp = start_stamp - else: - stamp = start_stamp + int((self._stamp_left - start_stamp) / stamp_step) * stamp_step + stamp_step - - while stamp < self._stamp_right: - yield stamp - stamp += stamp_step - - def _get_label(self, division, elapsed): - """ - :param division: number of seconds in a division, ''int'' - :param elapsed: seconds from the beginning, ''int'' - :returns: relevent time elapsed string, ''str'' - """ - secs = int(elapsed) % 60 - - mins = int(elapsed) / 60 - hrs = mins / 60 - days = hrs / 24 - weeks = days / 7 - - if division >= 7 * 24 * 60 * 60: # >1wk divisions: show weeks - return '%dw' % weeks - elif division >= 24 * 60 * 60: # >24h divisions: show days - return '%dd' % days - elif division >= 60 * 60: # >1h divisions: show hours - return '%dh' % hrs - elif division >= 5 * 60: # >5m divisions: show minutes - return '%dm' % mins - elif division >= 1: # >1s divisions: show minutes:seconds - return '%dm%02ds' % (mins, secs) - elif division >= 0.1: # >0.1s divisions: show seconds.0 - return '%d.%ss' % (secs, str(int(10.0 * (elapsed - int(elapsed))))) - elif division >= 0.01: # >0.1s divisions: show seconds.0 - return '%d.%02ds' % (secs, int(100.0 * (elapsed - int(elapsed)))) - else: # show seconds.00 - return '%d.%03ds' % (secs, int(1000.0 * (elapsed - int(elapsed)))) - - # Pixel location/time conversion functions - def map_x_to_stamp(self, x, clamp_to_visible=True): - """ - converts a pixel x value to a stamp - :param x: pixel value to be converted, ''int'' - :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool'' - :returns: timestamp, ''int'' - """ - fraction = float(x - self._history_left) / self._history_width - - if clamp_to_visible: - if fraction <= 0.0: - return self._stamp_left - elif fraction >= 1.0: - return self._stamp_right - - return self._stamp_left + fraction * (self._stamp_right - self._stamp_left) - - def map_dx_to_dstamp(self, dx): - """ - converts a distance in pixel space to a distance in stamp space - :param dx: distance in pixel space to be converted, ''int'' - :returns: distance in stamp space, ''float'' - """ - return float(dx) * (self._stamp_right - self._stamp_left) / self._history_width - - def map_stamp_to_x(self, stamp, clamp_to_visible=True): - """ - converts a timestamp to the x value where that stamp exists in the timeline - :param stamp: timestamp to be converted, ''int'' - :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool'' - :returns: # of pixels from the left boarder, ''int'' - """ - if self._stamp_left is None: - return None - fraction = (stamp - self._stamp_left) / (self._stamp_right - self._stamp_left) - - if clamp_to_visible: - fraction = min(1.0, max(0.0, fraction)) - - return self._history_left + fraction * self._history_width - - def map_dstamp_to_dx(self, dstamp): - return (float(dstamp) * self._history_width) / (self._stamp_right - self._stamp_left) - - def map_y_to_topic(self, y): - for topic in self._history_bounds: - x, topic_y, w, topic_h = self._history_bounds[topic] - if y > topic_y and y <= topic_y + topic_h: - return topic - return None - - # View port manipulation functions - def reset_timeline(self): - self.reset_zoom() - - self._selected_left = None - self._selected_right = None - self._selecting_mode = _SelectionMode.NONE - - self.emit_play_region() - - if self._stamp_left is not None: - self.playhead = rospy.Time.from_sec(self._stamp_left) - - def set_timeline_view(self, stamp_left, stamp_right): - self._stamp_left = stamp_left - self._stamp_right = stamp_right - - def translate_timeline(self, dstamp): - self.set_timeline_view(self._stamp_left + dstamp, self._stamp_right + dstamp) - self.scene().update() - - def translate_timeline_left(self): - self.translate_timeline((self._stamp_right - self._stamp_left) * -0.05) - - def translate_timeline_right(self): - self.translate_timeline((self._stamp_right - self._stamp_left) * 0.05) - - # Zoom functions - def reset_zoom(self): - start_stamp, end_stamp = self._start_stamp, self._end_stamp - if start_stamp is None: - return - - if (end_stamp - start_stamp) < rospy.Duration.from_sec(5.0): - end_stamp = start_stamp + rospy.Duration.from_sec(5.0) - - self.set_timeline_view(start_stamp.to_sec(), end_stamp.to_sec()) - self.scene().update() - - def zoom_in(self): - self.zoom_timeline(0.5) - - def zoom_out(self): - self.zoom_timeline(2.0) - - def can_zoom_in(self): - return self.can_zoom(0.5) - - def can_zoom_out(self): - return self.can_zoom(2.0) - - def can_zoom(self, desired_zoom): - if not self._stamp_left or not self.playhead: - return False - - new_interval = self.get_zoom_interval(desired_zoom) - if not new_interval: - return False - - new_range = new_interval[1] - new_interval[0] - curr_range = self._stamp_right - self._stamp_left - actual_zoom = new_range / curr_range - - if desired_zoom < 1.0: - return actual_zoom < 0.95 - else: - return actual_zoom > 1.05 - - def zoom_timeline(self, zoom, center=None): - interval = self.get_zoom_interval(zoom, center) - if not interval: - return - - self._stamp_left, self._stamp_right = interval - - self.scene().update() - - def get_zoom_interval(self, zoom, center=None): - """ - @rtype: tuple - @requires: left & right zoom interval sizes. - """ - if self._stamp_left is None: - return None - - stamp_interval = self._stamp_right - self._stamp_left - if center is None: - center = self.playhead.to_sec() - center_frac = (center - self._stamp_left) / stamp_interval - - new_stamp_interval = zoom * stamp_interval - if new_stamp_interval == 0: - return None - # Enforce zoom limits - px_per_sec = self._history_width / new_stamp_interval - if px_per_sec < self._min_zoom: - new_stamp_interval = self._history_width / self._min_zoom - elif px_per_sec > self._max_zoom: - new_stamp_interval = self._history_width / self._max_zoom - - left = center - center_frac * new_stamp_interval - right = left + new_stamp_interval - - return (left, right) - - def pause(self): - self._paused = True - - def resume(self): - self._paused = False - self._bag_timeline.resume() - - # Mouse event handlers - def on_middle_down(self, event): - self._clicked_pos = self._dragged_pos = event.pos() - self.pause() - - def on_left_down(self, event): - if self.playhead == None: - return - - self._clicked_pos = self._dragged_pos = event.pos() - - self.pause() - - if event.modifiers() == Qt.ShiftModifier: - return - - x = self._clicked_pos.x() - y = self._clicked_pos.y() - if x >= self._history_left and x <= self._history_right: - if y >= self._history_top and y <= self._history_bottom: - # Clicked within timeline - set playhead - playhead_secs = self.map_x_to_stamp(x) - if playhead_secs <= 0.0: - self.playhead = rospy.Time(0, 1) - else: - self.playhead = rospy.Time.from_sec(playhead_secs) - self.scene().update() - - elif y <= self._history_top: - # Clicked above timeline - if self._selecting_mode == _SelectionMode.NONE: - self._selected_left = None - self._selected_right = None - self._selecting_mode = _SelectionMode.LEFT_MARKED - self.scene().update() - self.emit_play_region() - - elif self._selecting_mode == _SelectionMode.MARKED: - left_x = self.map_stamp_to_x(self._selected_left) - right_x = self.map_stamp_to_x(self._selected_right) - if x < left_x - self._selection_handle_width or x > right_x + self._selection_handle_width: - self._selected_left = None - self._selected_right = None - self._selecting_mode = _SelectionMode.LEFT_MARKED - self.scene().update() - self.emit_play_region() - elif self._selecting_mode == _SelectionMode.SHIFTING: - self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor)) - - def on_mouse_up(self, event): - self.resume() - - if self._selecting_mode in [_SelectionMode.LEFT_MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]: - if self._selected_left is None: - self._selecting_mode = _SelectionMode.NONE - else: - self._selecting_mode = _SelectionMode.MARKED - self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor)) - self.scene().update() - - def on_mousewheel(self, event): - dz = event.delta() / 120.0 - self.zoom_timeline(1.0 - dz * 0.2) - - def on_mouse_move(self, event): - if not self._history_left: # TODO: need a better notion of initialized - return - - x = event.pos().x() - y = event.pos().y() - - if event.buttons() == Qt.NoButton: - # Mouse moving - if self._selecting_mode in [_SelectionMode.MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]: - if y <= self._history_top and self._selected_left is not None: - left_x = self.map_stamp_to_x(self._selected_left) - right_x = self.map_stamp_to_x(self._selected_right) - - if abs(x - left_x) <= self._selection_handle_width: - self._selecting_mode = _SelectionMode.MOVE_LEFT - self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor)) - return - elif abs(x - right_x) <= self._selection_handle_width: - self._selecting_mode = _SelectionMode.MOVE_RIGHT - self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor)) - return - elif x > left_x and x < right_x: - self._selecting_mode = _SelectionMode.SHIFTING - self.scene().views()[0].setCursor(QCursor(Qt.OpenHandCursor)) - return - else: - self._selecting_mode = _SelectionMode.MARKED - self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor)) - else: - # Mouse dragging - if event.buttons() == Qt.MidButton or event.modifiers() == Qt.ShiftModifier: - # Middle or shift: zoom and pan - dx_drag, dy_drag = x - self._dragged_pos.x(), y - self._dragged_pos.y() - - if dx_drag != 0: - self.translate_timeline(-self.map_dx_to_dstamp(dx_drag)) - if (dx_drag == 0 and abs(dy_drag) > 0) or (dx_drag != 0 and abs(float(dy_drag) / dx_drag) > 0.2 and abs(dy_drag) > 1): - zoom = min(self._max_zoom_speed, max(self._min_zoom_speed, 1.0 + self._zoom_sensitivity * dy_drag)) - self.zoom_timeline(zoom, self.map_x_to_stamp(x)) - - self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor)) - elif event.buttons() == Qt.LeftButton: - # Left: move selected region and move selected region boundry - clicked_x = self._clicked_pos.x() - clicked_y = self._clicked_pos.y() - - x_stamp = self.map_x_to_stamp(x) - - if y <= self._history_top: - if self._selecting_mode == _SelectionMode.LEFT_MARKED: - # Left and selecting: change selection region - clicked_x_stamp = self.map_x_to_stamp(clicked_x) - - self._selected_left = min(clicked_x_stamp, x_stamp) - self._selected_right = max(clicked_x_stamp, x_stamp) - self.scene().update() - - elif self._selecting_mode == _SelectionMode.MOVE_LEFT: - self._selected_left = x_stamp - self.scene().update() - - elif self._selecting_mode == _SelectionMode.MOVE_RIGHT: - self._selected_right = x_stamp - self.scene().update() - - elif self._selecting_mode == _SelectionMode.SHIFTING: - dx_drag = x - self._dragged_pos.x() - dstamp = self.map_dx_to_dstamp(dx_drag) - - self._selected_left = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_left + dstamp)) - self._selected_right = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_right + dstamp)) - self.scene().update() - self.emit_play_region() - - elif clicked_x >= self._history_left and clicked_x <= self._history_right and clicked_y >= self._history_top and clicked_y <= self._history_bottom: - # Left and clicked within timeline: change playhead - if x_stamp <= 0.0: - self.playhead = rospy.Time(0, 1) - else: - self.playhead = rospy.Time.from_sec(x_stamp) - self.scene().update() - self._dragged_pos = event.pos() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py deleted file mode 100644 index 4078f463..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py +++ /dev/null @@ -1,275 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QVBoxLayout, QMenu, QWidget, QDockWidget - -class TopicPopupWidget(QWidget): - def __init__(self, popup_name, timeline, viewer_type, topic): - super(TopicPopupWidget, self).__init__() - self.setObjectName(popup_name) - self.setWindowTitle(popup_name) - - layout = QVBoxLayout() - self.setLayout(layout) - - self._timeline = timeline - self._viewer_type = viewer_type - self._topic = topic - self._viewer = None - self._is_listening = False - - def hideEvent(self, event): - if self._is_listening: - self._timeline.remove_listener(self._topic, self._viewer) - self._is_listening = False - super(TopicPopupWidget, self).hideEvent(event) - - def showEvent(self, event): - if not self._is_listening: - self._timeline.add_listener(self._topic, self._viewer) - self._is_listening = True - super(TopicPopupWidget, self).showEvent(event) - - def show(self, context): - """ - Make this topic popup visible, if necessary. This includes setting up - the proper close button hacks - """ - if not self.parent(): - context.add_widget(self) - # make the dock widget closable, even if it normally isn't - dock_features = self.parent().features() - dock_features |= QDockWidget.DockWidgetClosable - self.parent().setFeatures(dock_features) - - # remove old listener - if self._viewer: - self._timeline.remove_listener(self._topic, self._viewer) - self._viewer = None - - # clean out the layout - while self.layout().count() > 0: - item = self.layout().itemAt(0) - self.layout().removeItem(item) - - # create a new viewer - self._viewer = self._viewer_type(self._timeline, self, self._topic) - if not self._is_listening: - self._timeline.add_listener(self._topic, self._viewer) - self._is_listening = True - - super(TopicPopupWidget, self).show() - -class TimelinePopupMenu(QMenu): - """ - Custom popup menu displayed on rightclick from timeline - """ - def __init__(self, timeline, event, menu_topic): - super(TimelinePopupMenu, self).__init__() - - self.parent = timeline - self.timeline = timeline - - - if menu_topic is not None: - self.setTitle(menu_topic) - self._menu_topic = menu_topic - else: - self._menu_topic = None - - self._reset_timeline = self.addAction('Reset Timeline') - - self._play_all = self.addAction('Play All Messages') - self._play_all.setCheckable(True) - self._play_all.setChecked(self.timeline.play_all) - - self.addSeparator() - - self._renderers = self.timeline._timeline_frame.get_renderers() - self._thumbnail_actions = [] - - # create thumbnail menu items - if menu_topic is None: - submenu = self.addMenu('Thumbnails...') - self._thumbnail_show_action = submenu.addAction('Show All') - self._thumbnail_hide_action = submenu.addAction('Hide All') - submenu.addSeparator() - - for topic, renderer in self._renderers: - self._thumbnail_actions.append(submenu.addAction(topic)) - self._thumbnail_actions[-1].setCheckable(True) - self._thumbnail_actions[-1].setChecked(self.timeline._timeline_frame.is_renderer_active(topic)) - else: - self._thumbnail_show_action = None - self._thumbnail_hide_action = None - for topic, renderer in self._renderers: - if menu_topic == topic: - self._thumbnail_actions.append(self.addAction("Thumbnail")) - self._thumbnail_actions[-1].setCheckable(True) - self._thumbnail_actions[-1].setChecked(self.timeline._timeline_frame.is_renderer_active(topic)) - - # create view menu items - self._topic_actions = [] - self._type_actions = [] - if menu_topic is None: - self._topics = self.timeline._timeline_frame.topics - view_topics_menu = self.addMenu('View (by Topic)') - for topic in self._topics: - datatype = self.timeline.get_datatype(topic) - - # View... / topic - topic_menu = QMenu(topic, self) - viewer_types = self.timeline._timeline_frame.get_viewer_types(datatype) - - # View... / topic / Viewer - for viewer_type in viewer_types: - tempaction = topic_menu.addAction(viewer_type.name) - tempaction.setData(viewer_type) - self._topic_actions.append(tempaction) - view_topics_menu.addMenu(topic_menu) - - view_type_menu = self.addMenu('View (by Type)') - self._topics_by_type = self.timeline._timeline_frame._topics_by_datatype - for datatype in self._topics_by_type: - # View... / datatype - datatype_menu = QMenu(datatype, self) - datatype_topics = self._topics_by_type[datatype] - viewer_types = self.timeline._timeline_frame.get_viewer_types(datatype) - for topic in [t for t in self._topics if t in datatype_topics]: # use timeline ordering - topic_menu = QMenu(topic, datatype_menu) - # View... / datatype / topic / Viewer - for viewer_type in viewer_types: - tempaction = topic_menu.addAction(viewer_type.name) - tempaction.setData(viewer_type) - self._topic_actions.append(tempaction) - datatype_menu.addMenu(topic_menu) - view_type_menu.addMenu(datatype_menu) - else: - view_menu = self.addMenu("View") - datatype = self.timeline.get_datatype(menu_topic) - - viewer_types = self.timeline._timeline_frame.get_viewer_types(datatype) - for viewer_type in viewer_types: - tempaction = view_menu.addAction(viewer_type.name) - tempaction.setData(viewer_type) - self._topic_actions.append(tempaction) - - self.addSeparator() - - # create publish menu items - self._publish_actions = [] - if menu_topic is None: - submenu = self.addMenu('Publish...') - - self._publish_all = submenu.addAction('Publish All') - self._publish_none = submenu.addAction('Publish None') - - submenu.addSeparator() - - for topic in self._topics: - self._publish_actions.append(submenu.addAction(topic)) - self._publish_actions[-1].setCheckable(True) - self._publish_actions[-1].setChecked(self.timeline.is_publishing(topic)) - else: - self._publish_actions.append(self.addAction("Publish")) - self._publish_actions[-1].setCheckable(True) - self._publish_actions[-1].setChecked(self.timeline.is_publishing(menu_topic)) - self._publish_all = None - self._publish_none = None - - - - action = self.exec_(event.globalPos()) - if action is not None and action != 0: - self.process(action) - - def process(self, action): - """ - :param action: action to execute, ''QAction'' - :raises: when it doesn't recognice the action passed in, ''Exception'' - """ - if action == self._reset_timeline: - self.timeline._timeline_frame.reset_timeline() - elif action == self._play_all: - self.timeline.toggle_play_all() - elif action == self._publish_all: - for topic in self.timeline._timeline_frame.topics: - if not self.timeline.start_publishing(topic): - break - elif action == self._publish_none: - for topic in self.timeline._timeline_frame.topics: - if not self.timeline.stop_publishing(topic): - break - elif action == self._thumbnail_show_action: - self.timeline._timeline_frame.set_renderers_active(True) - elif action == self._thumbnail_hide_action: - self.timeline._timeline_frame.set_renderers_active(False) - elif action in self._thumbnail_actions: - if self._menu_topic is None: - topic = action.text() - else: - topic = self._menu_topic - - if self.timeline._timeline_frame.is_renderer_active(topic): - self.timeline._timeline_frame.set_renderer_active(topic, False) - else: - self.timeline._timeline_frame.set_renderer_active(topic, True) - elif action in self._topic_actions + self._type_actions: - if self._menu_topic is None: - topic = action.parentWidget().title() - else: - topic = self._menu_topic - - popup_name = topic + '__' + action.text() - if popup_name not in self.timeline.popups: - frame = TopicPopupWidget(popup_name, self.timeline, - action.data(), str(topic)) - - self.timeline.add_view(topic, frame) - self.timeline.popups[popup_name] = frame - - # make popup visible - frame = self.timeline.popups[popup_name] - frame.show(self.timeline.get_context()) - - elif action in self._publish_actions: - if self._menu_topic is None: - topic = action.text() - else: - topic = self._menu_topic - - if self.timeline.is_publishing(topic): - self.timeline.stop_publishing(topic) - else: - self.timeline.start_publishing(topic) - else: - raise Exception('Unknown action in TimelinePopupMenu.process') diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py deleted file mode 100644 index 75d189a8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py +++ /dev/null @@ -1,112 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2014, Surya Ambrose, Aldebaran -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rosgraph - -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QWidget, QHBoxLayout, QVBoxLayout, QCheckBox, QScrollArea, QPushButton - -class TopicSelection(QWidget): - - recordSettingsSelected = Signal(bool, list) - - def __init__(self): - super(TopicSelection, self).__init__() - master = rosgraph.Master('rqt_bag_recorder') - self.setWindowTitle("Select the topics you want to record") - self.resize(500, 700) - - self.topic_list = [] - self.selected_topics = [] - self.items_list = [] - - self.area = QScrollArea(self) - self.main_widget = QWidget(self.area) - self.ok_button = QPushButton("Record", self) - self.ok_button.clicked.connect(self.onButtonClicked) - self.ok_button.setEnabled(False) - - self.main_vlayout = QVBoxLayout(self) - self.main_vlayout.addWidget(self.area) - self.main_vlayout.addWidget(self.ok_button) - self.setLayout(self.main_vlayout) - - self.selection_vlayout = QVBoxLayout(self) - self.item_all = QCheckBox("All", self) - self.item_all.stateChanged.connect(self.updateList) - self.selection_vlayout.addWidget(self.item_all) - topic_data_list = master.getPublishedTopics('') - topic_data_list.sort() - for topic, datatype in topic_data_list: - self.addCheckBox(topic) - - self.main_widget.setLayout(self.selection_vlayout) - - self.area.setWidget(self.main_widget) - self.show() - - def addCheckBox(self, topic): - self.topic_list.append(topic) - item = QCheckBox(topic, self) - item.stateChanged.connect(lambda x: self.updateList(x,topic)) - self.items_list.append(item) - self.selection_vlayout.addWidget(item) - - - def updateList(self, state, topic = None): - if topic is None: # The "All" checkbox was checked / unchecked - if state == Qt.Checked: - self.item_all.setTristate(False) - for item in self.items_list: - if item.checkState() == Qt.Unchecked: - item.setCheckState(Qt.Checked) - elif state == Qt.Unchecked: - self.item_all.setTristate(False) - for item in self.items_list: - if item.checkState() == Qt.Checked: - item.setCheckState(Qt.Unchecked) - else: - if state == Qt.Checked: - self.selected_topics.append(topic) - else: - self.selected_topics.remove(topic) - if self.item_all.checkState()==Qt.Checked: - self.item_all.setCheckState(Qt.PartiallyChecked) - - if self.selected_topics == []: - self.ok_button.setEnabled(False) - else: - self.ok_button.setEnabled(True) - - def onButtonClicked(self): - self.close() - self.recordSettingsSelected.emit((self.item_all.checkState() == Qt.Checked), self.selected_topics) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst deleted file mode 100644 index 8128b90f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst +++ /dev/null @@ -1,109 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_bag_plugins -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* add missing dependency on rqt_plot (`#316 `_) -* work around Pillow segfault if PyQt5 is installed (`#289 `_, `#290 `_) - -0.3.10 (2014-10-01) -------------------- -* add displaying of depth image thumbnails - -0.3.9 (2014-08-18) ------------------- -* add missing dependency on python-cairo (`#269 `_) - -0.3.8 (2014-07-15) ------------------- -* fix missing installation of resource subfolder - -0.3.7 (2014-07-11) ------------------- -* add plotting plugin (`#239 `_) -* fix rqt_bag to plot array members (`#253 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* fix PIL/Pillow error (`#224 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into Groovy diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt deleted file mode 100644 index 6561ffff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_bag_plugins) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox deleted file mode 100644 index 6b5645f7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_bag_plugins - - - ---> - - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml b/workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml deleted file mode 100644 index 43113cfd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - rqt_bag_plugins - 0.3.13 - rqt_bag provides a GUI plugin for displaying and replaying ROS bag files. - Aaron Blasdel - Austin Hendrix - - BSD - - http://ros.org/wiki/rqt_bag - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - Tim Field - - catkin - - geometry_msgs - python-cairo - python-imaging - rosbag - roslib - rospy - rqt_bag - rqt_gui - rqt_gui_py - rqt_plot - sensor_msgs - std_msgs - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml b/workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml deleted file mode 100644 index bac205d5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - A plugin for rqt_bag for showing images. - - - - - - A plugin for rqt_bag for plotting data. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui b/workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui deleted file mode 100644 index bd7e6fdb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui +++ /dev/null @@ -1,162 +0,0 @@ - - - Form - - - - 0 - 0 - 488 - 329 - - - - - 0 - 0 - - - - Form - - - Qt::LeftToRight - - - - - - - 0 - 0 - - - - Qt::LeftToRight - - - Qt::Horizontal - - - - - 200 - 0 - - - - - 200 - 0 - - - - - 0 - 0 - - - - - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - - - - - - - - - - - Resolution: - - - - - - - - - - - false - - - - 0 - 0 - - - - - 100 - 16777215 - - - - - - - 100 - - - true - - - - - - - auto - - - true - - - true - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py deleted file mode 100644 index 2b2255fb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_bag_plugins'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/__init__.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py deleted file mode 100755 index ce3d5d23..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py +++ /dev/null @@ -1,102 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import array -from cStringIO import StringIO -import sys - -from PIL import Image -from PIL import ImageOps -import cairo - - -def imgmsg_to_pil(img_msg, rgba=True): - try: - if img_msg._type == 'sensor_msgs/CompressedImage': - pil_img = Image.open(StringIO(img_msg.data)) - if pil_img.mode != 'L': - pil_img = pil_bgr2rgb(pil_img) - else: - alpha = False - pil_mode = 'RGB' - if img_msg.encoding == 'mono8': - mode = 'L' - elif img_msg.encoding == 'rgb8': - mode = 'RGB' - elif img_msg.encoding == 'bgr8': - mode = 'BGR' - elif img_msg.encoding in ['bayer_rggb8', 'bayer_bggr8', 'bayer_gbrg8', 'bayer_grbg8']: - mode = 'L' - elif img_msg.encoding == 'mono16' or img_msg.encoding == '16UC1': - pil_mode = 'F' - if img_msg.is_bigendian: - mode = 'F;16B' - else: - mode = 'F;16' - elif img_msg.encoding == 'rgba8': - mode = 'BGR' - alpha = True - elif img_msg.encoding == 'bgra8': - mode = 'RGB' - alpha = True - else: - raise Exception("Unsupported image format: %s" % img_msg.encoding) - pil_img = Image.frombuffer(pil_mode, (img_msg.width, img_msg.height), img_msg.data, 'raw', mode, 0, 1) - - # 16 bits conversion to 8 bits - if pil_img.mode == 'F': - pil_img = pil_img.point(lambda i: i*(1./256.)).convert('L') - pil_img = ImageOps.autocontrast(pil_img) - - if rgba and pil_img.mode != 'RGBA': - pil_img = pil_img.convert('RGBA') - - return pil_img - - except Exception, ex: - print >> sys.stderr, 'Can\'t convert image: %s' % ex - return None - - -def pil_bgr2rgb(pil_img): - rgb2bgr = (0, 0, 1, 0, - 0, 1, 0, 0, - 1, 0, 0, 0) - return pil_img.convert('RGB', rgb2bgr) - - -def pil_to_cairo(pil_img): - w, h = pil_img.size - data = array.array('c') - data.fromstring(pil_img.tostring()) - - return cairo.ImageSurface.create_for_data(data, cairo.FORMAT_ARGB32, w, h) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py deleted file mode 100755 index 0a9bf63c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py +++ /dev/null @@ -1,51 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_bag.plugins.plugin import Plugin - -from .image_timeline_renderer import ImageTimelineRenderer -from .image_view import ImageView - - -class ImagePlugin(Plugin): - - def __init__(self): - pass - - def get_view_class(self): - return ImageView - - def get_renderer_class(self): - return ImageTimelineRenderer - - def get_message_types(self): - return ['sensor_msgs/Image', 'sensor_msgs/CompressedImage'] diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py deleted file mode 100755 index cbbd9628..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py +++ /dev/null @@ -1,181 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy - -# HACK workaround for upstream pillow issue python-pillow/Pillow#400 -import sys -if 'PyQt5' in sys.modules: - sys.modules['PyQt5'] = None -from PIL import Image -from PIL.ImageQt import ImageQt - -from rqt_bag import TimelineCache, TimelineRenderer - -import image_helper - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QBrush, QPen, QPixmap - - -class ImageTimelineRenderer(TimelineRenderer): - """ - Draws thumbnails of sensor_msgs/Image or sensor_msgs/CompressedImage in the timeline. - """ - def __init__(self, timeline, thumbnail_height=160): - super(ImageTimelineRenderer, self).__init__(timeline, msg_combine_px=40.0) - - self.thumbnail_height = thumbnail_height - - self.thumbnail_combine_px = 20.0 # use cached thumbnail if it's less than this many pixels away - self.min_thumbnail_width = 8 # don't display thumbnails if less than this many pixels across - self.quality = Image.NEAREST # quality hint for thumbnail scaling - - self.thumbnail_cache = TimelineCache(self._load_thumbnail, lambda topic, msg_stamp, thumbnail: self.timeline.scene().update()) - # TimelineRenderer implementation - - def get_segment_height(self, topic): - return self.thumbnail_height - - def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height): - """ - draws a stream of images for the topic - :param painter: painter object, ''QPainter'' - :param topic: topic to draw, ''str'' - :param stamp_start: stamp to start drawing, ''rospy.Time'' - :param stamp_end: stamp to end drawing, ''rospy.Time'' - :param x: x to draw images at, ''int'' - :param y: y to draw images at, ''int'' - :param width: width in pixels of the timeline area, ''int'' - :param height: height in pixels of the timeline area, ''int'' - """ - if x < self.timeline._history_left: - width += x - self.timeline._history_left - x = self.timeline._history_left - max_interval_thumbnail = self.timeline.map_dx_to_dstamp(self.thumbnail_combine_px) - max_interval_thumbnail = max(0.1, max_interval_thumbnail) - thumbnail_gap = 6 - thumbnail_x, thumbnail_y, thumbnail_height = x + 1, y + 1, height - 2 - thumbnail_gap # leave 1px border - - # set color to white draw rectangle over messages - painter.setBrush(QBrush(Qt.white)) - painter.drawRect(x, y, width, height - thumbnail_gap) - thumbnail_width = None - - while True: - available_width = (x + width) - thumbnail_x - - # Check for enough remaining to draw thumbnail - if width > 1 and available_width < self.min_thumbnail_width: - break - - # Try to display the thumbnail, if its right edge is to the right of the timeline's left side - if not thumbnail_width or thumbnail_x + thumbnail_width >= self.timeline._history_left: - stamp = self.timeline.map_x_to_stamp(thumbnail_x, clamp_to_visible=False) - thumbnail_bitmap = self.thumbnail_cache.get_item(topic, stamp, max_interval_thumbnail) - - # Cache miss - if not thumbnail_bitmap: - thumbnail_details = (thumbnail_height,) - self.thumbnail_cache.enqueue((topic, stamp, max_interval_thumbnail, thumbnail_details)) - if not thumbnail_width: - break - else: - thumbnail_width, _ = thumbnail_bitmap.size - - if width > 1: - if available_width < thumbnail_width: - thumbnail_width = available_width - 1 - QtImage = ImageQt(thumbnail_bitmap) - pixmap = QPixmap.fromImage(QtImage) - painter.drawPixmap(thumbnail_x, thumbnail_y, thumbnail_width, thumbnail_height, pixmap) - thumbnail_x += thumbnail_width - - if width == 1: - break - - painter.setPen(QPen(QBrush(Qt.black))) - painter.setBrush(QBrush(Qt.transparent)) - if width == 1: - painter.drawRect(x, y, thumbnail_x - x, height - thumbnail_gap - 1) - else: - painter.drawRect(x, y, width, height - thumbnail_gap - 1) - return True - - def close(self): - if self.thumbnail_cache: - self.thumbnail_cache.stop() - self.thumbnail_cache.join() - - def _load_thumbnail(self, topic, stamp, thumbnail_details): - """ - Loads the thumbnail from the bag - """ - (thumbnail_height,) = thumbnail_details - - # Find position of stamp using index - t = rospy.Time.from_sec(stamp) - bag, entry = self.timeline.scene().get_entry(t, topic) - if not entry: - return None, None - pos = entry.position - - # Not in the cache; load from the bag file - - with self.timeline.scene()._bag_lock: - msg_topic, msg, msg_stamp = bag._read_message(pos) - - # Convert from ROS image to PIL image - try: - pil_image = image_helper.imgmsg_to_pil(msg) - except Exception, ex: - print >> sys.stderr, 'Error loading image on topic %s: %s' % (topic, str(ex)) - pil_image = None - - if not pil_image: - print >> sys.stderr, 'Disabling renderer on %s' % topic - self.timeline.set_renderer_active(topic, False) - return None, None - - # Calculate width to maintain aspect ratio - try: - pil_image_size = pil_image.size - thumbnail_width = int(round(thumbnail_height * (float(pil_image_size[0]) / pil_image_size[1]))) - # Scale to thumbnail size - thumbnail = pil_image.resize((thumbnail_width, thumbnail_height), self.quality) - - return msg_stamp, thumbnail - - except Exception, ex: - print >> sys.stderr, 'Error loading image on topic %s: %s' % (topic, str(ex)) - raise - return None, None diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py deleted file mode 100755 index e056abb9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py +++ /dev/null @@ -1,111 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from PIL import Image - -# HACK workaround for upstream pillow issue python-pillow/Pillow#400 -import sys -if 'PyQt5' in sys.modules: - sys.modules['PyQt5'] = None -from PIL.ImageQt import ImageQt - -from rqt_bag import TopicMessageView -import image_helper - -from python_qt_binding.QtGui import QGraphicsScene, QGraphicsView, QPixmap - - -class ImageView(TopicMessageView): - """ - Popup image viewer - """ - name = 'Image' - - def __init__(self, timeline, parent, topic): - super(ImageView, self).__init__(timeline, parent, topic) - - self._image = None - self._image_topic = None - self._image_stamp = None - self.quality = Image.NEAREST # quality hint for scaling - - # TODO put the image_topic and image_stamp on the picture or display them in some fashion - self._overlay_font_size = 14.0 - self._overlay_indent = (4, 4) - self._overlay_color = (0.2, 0.2, 1.0) - - self._image_view = QGraphicsView(parent) - self._image_view.resizeEvent = self._resizeEvent - self._scene = QGraphicsScene() - self._image_view.setScene(self._scene) - parent.layout().addWidget(self._image_view) - - # MessageView implementation - def _resizeEvent(self, event): - # TODO make this smarter. currently there will be no scrollbar even if the timeline extends beyond the viewable area - self._scene.setSceneRect(0, 0, self._image_view.size().width() - 2, self._image_view.size().height() - 2) - self.put_image_into_scene() - - def message_viewed(self, bag, msg_details): - """ - refreshes the image - """ - TopicMessageView.message_viewed(self, bag, msg_details) - topic, msg, t = msg_details[:3] - if not msg: - self.set_image(None, topic, 'no message') - else: - self.set_image(msg, topic, msg.header.stamp) - - def message_cleared(self): - TopicMessageView.message_cleared(self) - self.set_image(None, None, None) - - # End MessageView implementation - def put_image_into_scene(self): - if self._image: - resized_image = self._image.resize((self._image_view.size().width() - 2, self._image_view.size().height() - 2), self.quality) - - QtImage = ImageQt(resized_image) - pixmap = QPixmap.fromImage(QtImage) - self._scene.clear() - self._scene.addPixmap(pixmap) - - def set_image(self, image_msg, image_topic, image_stamp): - self._image_msg = image_msg - if image_msg: - self._image = image_helper.imgmsg_to_pil(image_msg) - else: - self._image = None - self._image_topic = image_topic - self._image_stamp = image_stamp - self.put_image_into_scene() diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py deleted file mode 100755 index eddfb087..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py +++ /dev/null @@ -1,50 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_bag.plugins.plugin import Plugin - -from .plot_view import PlotView - - -class PlotPlugin(Plugin): - - def __init__(self): - pass - - def get_view_class(self): - return PlotView - - def get_renderer_class(self): - return None - - def get_message_types(self): - return ['*'] diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py deleted file mode 100755 index a50b7eb8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py +++ /dev/null @@ -1,495 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2014, Austin Hendrix, Stanford University -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Design notes: -# -# The original rxbag plot widget displays the plot -# -# It has a settings menu which allows the user to add subplots -# and assign arbitrary fields to subplots. possibly many fields in a single -# subplot, or one field per subplot, or any othe rcombination -# in particular, it is possible to add the same field to multiple subplots -# It doesn't appear able to add fields from different topics to the same plot -# Since rqt_plot can do this, and it's useful for our application, it's worth -# thinking about. If it makes the UI too cluttered, it may not be worth it -# If it isn't possible due to the architecture of rqt_bag, it may not be -# worth it -# -# Thoughts on new design: -# a plottable field is anything which is either numeric, or contains -# at least one plottable subfield (this is useful for enabling/disabling all -# of the fields of a complex type) -# -# for simple messages, we could display a tree view of the message fields -# on top of the plot, along with the color for each plottable field. This gets -# unweildy for large messages, because they'll use up too much screen space -# displaying the topic list -# -# It may be better to display the topic list for a plot as a dockable widget, -# and be able to dismiss it when it isn't actively in use, similar to the -# existing rxbag plot config window -# -# The plot should be dockable and viewable. If it's a separate window, it -# doesn't make sense to make it align with the timeline. This could be done -# if someone wanted to implement a separate timeline view - -import os -import math -import codecs -import threading -import rospkg -from rqt_bag import MessageView - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, qWarning, Signal -from python_qt_binding.QtGui import QWidget, QPushButton, QTreeWidget, QTreeWidgetItem, QSizePolicy, QDoubleValidator, QIcon - -from rqt_plot.data_plot import DataPlot - -# rospy used for Time and Duration objects, for interacting with rosbag -import rospy - -class PlotView(MessageView): - """ - Popup plot viewer - """ - name = 'Plot' - - def __init__(self, timeline, parent, topic): - super(PlotView, self).__init__(timeline, topic) - - self.plot_widget = PlotWidget(timeline, parent, topic) - - parent.layout().addWidget(self.plot_widget) - - def message_viewed(self, bag, msg_details): - """ - refreshes the plot - """ - _, msg, t = msg_details[:3] - - if t is None: - self.message_cleared() - else: - self.plot_widget.message_tree.set_message(msg) - self.plot_widget.set_cursor((t-self.plot_widget.start_stamp).to_sec()) - - def message_cleared(self): - pass - -class PlotWidget(QWidget): - - def __init__(self, timeline, parent, topic): - super(PlotWidget, self).__init__(parent) - self.setObjectName('PlotWidget') - - self.timeline = timeline - msg_type = self.timeline.get_datatype(topic) - self.msgtopic = topic - self.start_stamp = self.timeline._get_start_stamp() - self.end_stamp = self.timeline._get_end_stamp() - - # the current region-of-interest for our bag file - # all resampling and plotting is done with these limits - self.limits = [0,(self.end_stamp-self.start_stamp).to_sec()] - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_bag_plugins'), 'resource', 'plot.ui') - loadUi(ui_file, self) - self.message_tree = MessageTree(msg_type, self) - self.data_tree_layout.addWidget(self.message_tree) - # TODO: make this a dropdown with choices for "Auto", "Full" and - # "Custom" - # I continue to want a "Full" option here - self.auto_res.stateChanged.connect(self.autoChanged) - - self.resolution.editingFinished.connect(self.settingsChanged) - self.resolution.setValidator(QDoubleValidator(0.0,1000.0,6,self.resolution)) - - - self.timeline.selected_region_changed.connect(self.region_changed) - - self.recompute_timestep() - - self.plot = DataPlot(self) - self.plot.set_autoscale(x=False) - self.plot.set_autoscale(y=DataPlot.SCALE_VISIBLE) - self.plot.autoscroll(False) - self.plot.set_xlim(self.limits) - self.data_plot_layout.addWidget(self.plot) - - self._home_button = QPushButton() - self._home_button.setToolTip("Reset View") - self._home_button.setIcon(QIcon.fromTheme('go-home')) - self._home_button.clicked.connect(self.home) - self.plot_toolbar_layout.addWidget(self._home_button) - - self._config_button = QPushButton("Configure Plot") - self._config_button.clicked.connect(self.plot.doSettingsDialog) - self.plot_toolbar_layout.addWidget(self._config_button) - - self.set_cursor(0) - - self.paths_on = set() - self._lines = None - - # get bag from timeline - bag = None - start_time = self.start_stamp - while bag is None: - bag,entry = self.timeline.get_entry(start_time, topic) - if bag is None: - start_time = self.timeline.get_entry_after(start_time)[1].time - - self.bag = bag - # get first message from bag - msg = bag._read_message(entry.position) - self.message_tree.set_message(msg[1]) - - # state used by threaded resampling - self.resampling_active = False - self.resample_thread = None - self.resample_fields = set() - - def set_cursor(self, position): - self.plot.vline(position, color=DataPlot.RED) - self.plot.redraw() - - def add_plot(self, path): - self.resample_data([path]) - - def update_plot(self): - if len(self.paths_on)>0: - self.resample_data(self.paths_on) - - def remove_plot(self, path): - self.plot.remove_curve(path) - self.paths_on.remove(path) - self.plot.redraw() - - def load_data(self): - """get a generator for the specified time range on our bag""" - return self.bag.read_messages(self.msgtopic, - self.start_stamp+rospy.Duration.from_sec(self.limits[0]), - self.start_stamp+rospy.Duration.from_sec(self.limits[1])) - - def resample_data(self, fields): - if self.resample_thread: - # cancel existing thread and join - self.resampling_active = False - self.resample_thread.join() - - for f in fields: - self.resample_fields.add(f) - - # start resampling thread - self.resampling_active = True - self.resample_thread = threading.Thread(target=self._resample_thread) - # explicitly mark our resampling thread as a daemon, because we don't - # want to block program exit on a long resampling operation - self.resample_thread.setDaemon(True) - self.resample_thread.start() - - def _resample_thread(self): - # TODO: - # * look into doing partial display updates for long resampling - # operations - # * add a progress bar for resampling operations - x = {} - y = {} - for path in self.resample_fields: - x[path] = [] - y[path] = [] - - msgdata = self.load_data() - - for entry in msgdata: - # detect if we're cancelled and return early - if not self.resampling_active: - return - - for path in self.resample_fields: - # this resampling method is very unstable, because it picks - # representative points rather than explicitly representing - # the minimum and maximum values present within a sample - # If the data has spikes, this is particularly bad because they - # will be missed entirely at some resolutions and offsets - if x[path]==[] or (entry[2]-self.start_stamp).to_sec()-x[path][-1] >= self.timestep: - y_value = entry[1] - for field in path.split('.'): - index = None - if field.endswith(']'): - field = field[:-1] - field, _, index = field.rpartition('[') - y_value = getattr(y_value, field) - if index: - index = int(index) - y_value = y_value[index] - y[path].append(y_value) - x[path].append((entry[2]-self.start_stamp).to_sec()) - - # TODO: incremental plot updates would go here... - # we should probably do incremental updates based on time; - # that is, push new data to the plot maybe every .5 or .1 - # seconds - # time is a more useful metric than, say, messages loaded or - # percentage, because it will give a reasonable refresh rate - # without overloading the computer - # if we had a progress bar, we could emit a signal to update it here - - # update the plot with final resampled data - for path in self.resample_fields: - if len(x[path]) < 1: - qWarning("Resampling resulted in 0 data points for %s" % path) - else: - if path in self.paths_on: - self.plot.clear_values(path) - self.plot.update_values(path, x[path], y[path]) - else: - self.plot.add_curve(path, path, x[path], y[path]) - self.paths_on.add(path) - - self.plot.redraw() - - self.resample_fields.clear() - self.resampling_active = False - - def recompute_timestep(self): - # this is only called if we think the timestep has changed; either - # by changing the limits or by editing the resolution - limits = self.limits - if self.auto_res.isChecked(): - timestep = round((limits[1]-limits[0])/200.0,5) - else: - timestep = float(self.resolution.text()) - self.resolution.setText(str(timestep)) - self.timestep = timestep - - def region_changed(self, start, end): - # this is the only place where self.limits is set - limits = [ (start - self.start_stamp).to_sec(), - (end - self.start_stamp).to_sec() ] - - # cap the limits to the start and end of our bag file - if limits[0]<0: - limits = [0.0,limits[1]] - if limits[1]>(self.end_stamp-self.start_stamp).to_sec(): - limits = [limits[0],(self.end_stamp-self.start_stamp).to_sec()] - - self.limits = limits - - self.recompute_timestep() - self.plot.set_xlim(limits) - self.plot.redraw() - self.update_plot() - - def settingsChanged(self): - # resolution changed. recompute the timestep and resample - self.recompute_timestep() - self.update_plot() - - def autoChanged(self, state): - if state==2: - # auto mode enabled. recompute the timestep and resample - self.resolution.setDisabled(True) - self.recompute_timestep() - self.update_plot() - else: - # auto mode disabled. enable the resolution text box - # no change to resolution yet, so no need to redraw - self.resolution.setDisabled(False) - - def home(self): - # TODO: re-add the button for this. It's useful for restoring the - # X and Y limits so that we can see all of the data - # effectively a "zoom all" button - - # reset the plot to our current limits - self.plot.set_xlim(self.limits) - # redraw the plot; this forces a Y autoscaling - self.plot.redraw() - - - -class MessageTree(QTreeWidget): - def __init__(self, msg_type, parent): - super(MessageTree, self).__init__(parent) - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.setHeaderHidden(True) - self.itemChanged.connect(self.handleChanged) - self._msg_type = msg_type - self._msg = None - - self._expanded_paths = None - self._checked_states = set() - self.plot_list = set() - - # populate the tree from the message type - - - @property - def msg(self): - return self._msg - - def set_message(self, msg): - # Remember whether items were expanded or not before deleting - if self._msg: - for item in self.get_all_items(): - path = self.get_item_path(item) - if item.isExpanded(): - self._expanded_paths.add(path) - elif path in self._expanded_paths: - self._expanded_paths.remove(path) - if item.checkState(0)==Qt.Checked: - self._checked_states.add(path) - elif path in self._checked_states: - self._checked_states.remove(path) - self.clear() - if msg: - # Populate the tree - self._add_msg_object(None, '', '', msg, msg._type) - - if self._expanded_paths is None: - self._expanded_paths = set() - else: - # Expand those that were previously expanded, and collapse any paths that we've seen for the first time - for item in self.get_all_items(): - path = self.get_item_path(item) - if path in self._expanded_paths: - item.setExpanded(True) - else: - item.setExpanded(False) - self._msg = msg - self.update() - - def get_item_path(self, item): - return item.data(0, Qt.UserRole)[0].replace(' ', '') # remove spaces that may get introduced in indexing, e.g. [ 3] is [3] - - def get_all_items(self): - items = [] - try: - root = self.invisibleRootItem() - self.traverse(root, items.append) - except Exception: - # TODO: very large messages can cause a stack overflow due to recursion - pass - return items - - def traverse(self, root, function): - for i in range(root.childCount()): - child = root.child(i) - function(child) - self.traverse(child, function) - - def _add_msg_object(self, parent, path, name, obj, obj_type): - label = name - - if hasattr(obj, '__slots__'): - subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__] - elif type(obj) in [list, tuple]: - len_obj = len(obj) - if len_obj == 0: - subobjs = [] - else: - w = int(math.ceil(math.log10(len_obj))) - subobjs = [('[%*d]' % (w, i), subobj) for (i, subobj) in enumerate(obj)] - else: - subobjs = [] - - plotitem=False - if type(obj) in [int, long, float]: - plotitem=True - if type(obj) == float: - obj_repr = '%.6f' % obj - else: - obj_repr = str(obj) - - if obj_repr[0] == '-': - label += ': %s' % obj_repr - else: - label += ': %s' % obj_repr - - elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]: - # Ignore any binary data - obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0] - - # Truncate long representations - if len(obj_repr) >= 50: - obj_repr = obj_repr[:50] + '...' - - label += ': ' + obj_repr - item = QTreeWidgetItem([label]) - if name == '': - pass - elif path.find('.') == -1 and path.find('[') == -1: - self.addTopLevelItem(item) - else: - parent.addChild(item) - if plotitem == True: - if path.replace(' ', '') in self._checked_states: - item.setCheckState (0, Qt.Checked) - else: - item.setCheckState (0, Qt.Unchecked) - item.setData(0, Qt.UserRole, (path, obj_type)) - - - for subobj_name, subobj in subobjs: - if subobj is None: - continue - - if path == '': - subpath = subobj_name # root field - elif subobj_name.startswith('['): - subpath = '%s%s' % (path, subobj_name) # list, dict, or tuple - else: - subpath = '%s.%s' % (path, subobj_name) # attribute (prefix with '.') - - if hasattr(subobj, '_type'): - subobj_type = subobj._type - else: - subobj_type = type(subobj).__name__ - - self._add_msg_object(item, subpath, subobj_name, subobj, subobj_type) - - def handleChanged(self, item, column): - if item.data(0, Qt.UserRole)==None: - pass - else: - path = self.get_item_path(item) - if item.checkState(column) == Qt.Checked: - if path not in self.plot_list: - self.plot_list.add(path) - self.parent().parent().parent().add_plot(path) - if item.checkState(column) == Qt.Unchecked: - if path in self.plot_list: - self.plot_list.remove(path) - self.parent().parent().parent().remove_plot(path) diff --git a/workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst deleted file mode 100644 index fd6cdec4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst +++ /dev/null @@ -1,83 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_common_plugins -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- -* New pkgs added (rqt_action, rqt_launch) - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt deleted file mode 100644 index 3a2016cd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_common_plugins) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml b/workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml deleted file mode 100644 index 130d425f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml +++ /dev/null @@ -1,58 +0,0 @@ - - rqt_common_plugins - 0.3.13 - - rqt_common_plugins metapackage provides ROS backend graphical tools suite that can be used on/off of robot runtime.
-
- To run any rqt plugins, just type in a single command "rqt", then select any plugins you want from the GUI that launches afterwards.
-
- rqt consists of three following metapackages:
-
    -
  • rqt - core modules of rqt (ROS GUI) framework. rqt plugin developers barely needs to pay attention to this metapackage.
    • -
  • rqt_common_plugins (you're here!)
    • -
  • rqt_robot_plugins - rqt plugins that are particularly used with robots during their runtime.

    • -
-
- - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dirk Thomas - Dorian Scholz - Thibault Kruse - Aaron Blasdel - Isaac Saito - - catkin - rqt_action - rqt_bag - rqt_bag_plugins - rqt_console - rqt_dep - rqt_graph - rqt_image_view - rqt_launch - rqt_logger_level - - rqt_msg - rqt_plot - rqt_publisher - rqt_py_common - rqt_py_console - rqt_reconfigure - rqt_service_caller - rqt_shell - rqt_srv - rqt_top - rqt_topic - rqt_web - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst deleted file mode 100644 index 0e78a7db..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_console -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* use icons instead of text when available, refactor pause/resume button - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* rewrite of rqt_console to drastically improve performance (`#186 `_) - -0.3.0 (2013-08-28) ------------------- -* pause button no more saves state (`#125 `_) -* persist message limit (`#138 `_) -* add ability to set logger level (`#117 `_) -* add tooltips to table cells (`#143 `_) -* improve labels for filters (`#146 `_) -* fix time column when loading data from file (`#160 `_) -* fix applying message limit on change (`#133 `_) -* fix clear button to remove all messages (`#141 `_) -* fix sorting to use row index to decide order between equal values (except for time column) (`#124 `_) -* fix locking of message queue -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- -* added missing word in status tip - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- -* Fix; can't add filters when using pyside (`#36 `_) - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt deleted file mode 100644 index d7a78392..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_console) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_console - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_console/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_console/mainpage.dox deleted file mode 100644 index 085267a8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_console provides a GUI plugin for displaying and filtering ROS messages. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_console/package.xml b/workspace/src/rqt_common_plugins/rqt_console/package.xml deleted file mode 100644 index f8ccce80..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_console - 0.3.13 - rqt_console provides a GUI plugin for displaying and filtering ROS messages. - Aaron Blasdel - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_console - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - catkin - - python-rospkg - roslib - rospy - rqt_gui - rqt_gui_py - rqt_logger_level - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/plugin.xml b/workspace/src/rqt_common_plugins/rqt_console/plugin.xml deleted file mode 100644 index df26f202..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for displaying and filtering ROS messages. - - - - - folder - Plugins related to logging. - - - mail-message-new - A Python GUI plugin for displaying and filtering ROS log messages. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui deleted file mode 100644 index 684e9ca3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui +++ /dev/null @@ -1,120 +0,0 @@ - - - ConsoleSettingsDialog - - - Qt::ApplicationModal - - - - 0 - 0 - 368 - 123 - - - - Setup - - - true - - - - - - - - - 94 - 16777215 - - - - Rosout Topic - - - - - - - - - - - - - - - 94 - 16777215 - - - - Buffer Size - - - - - - - 100000 - - - 1000 - - - 20000 - - - - - - - - - - - - QDialogButtonBox::Cancel|QDialogButtonBox::Ok - - - - - - - - - buttonBox - accepted() - ConsoleSettingsDialog - accept() - - - 248 - 254 - - - 157 - 274 - - - - - buttonBox - rejected() - ConsoleSettingsDialog - reject() - - - 316 - 260 - - - 286 - 274 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui deleted file mode 100644 index 8b727160..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui +++ /dev/null @@ -1,340 +0,0 @@ - - - ConsoleWidget - - - - 0 - 0 - 838 - 675 - - - - Console - - - - 4 - - - 5 - - - - - QLayout::SetMinAndMaxSize - - - - - Load a Message file - - - Load - - - - 16 - 16 - - - - - - - - Save currently displayed messages to a file - - - Save - - - - 16 - 16 - - - - - - - - Pause the message stream - - - Pause - - - - 16 - 16 - - - - - - - - Resume the message stream - - - Resume - - - - 16 - 16 - - - - - - - - Displaying 0 messages - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Clear - - - - - - - Resizes message table columns to fit their contents exactly - - - Fit Columns - - - - - - - - - - - Qt::Vertical - - - 9 - - - true - - - - - - - true - - - QAbstractItemView::SelectRows - - - - 16 - 16 - - - - QAbstractItemView::ScrollPerPixel - - - true - - - false - - - false - - - - - - - - Qt::Vertical - - - 9 - - - true - - - - Messages matching ANY of these rules will NOT be displayed - - - Exclude Messages... - - - - - - true - - - QAbstractItemView::NoSelection - - - 1 - - - false - - - true - - - false - - - - - - - - - 27 - 16777215 - - - - Add new filter - - - - - - - 16 - 16 - - - - - - - - - Messages matching ANY of these rules will be highlighted - - - Highlight Messages... - - - - - - - - - true - - - QAbstractItemView::NoSelection - - - 1 - - - false - - - true - - - false - - - - - - - - - - - 27 - 16777215 - - - - Show only highlighted rows - - - - - - - 16 - 16 - - - - true - - - false - - - - - - - - 27 - 16777215 - - - - Add new filter - - - - - - - 16 - 16 - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui deleted file mode 100644 index 0aa39b27..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui +++ /dev/null @@ -1,188 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 125 - - - - Form - - - - 0 - - - - - 0 - - - - - 0 - - - - - Message: - - - - - - - Enter a Filter string - - - - - - - Toggle Regular Expressions - - - Regex - - - - - - - - - 0 - - - - - Severity: - - - - - - - - 16777215 - 45 - - - - Click a Severity to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - 20 - - - - - - - Topic - - - - - - - - 16777215 - 45 - - - - Click a Topic to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - - - 0 - - - - - Node - - - - - - - - 16777215 - 45 - - - - Click a Node to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui deleted file mode 100644 index b45a2855..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui +++ /dev/null @@ -1,73 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - Toggle Filter Enabled - - - true - - - - - - - Name - - - - - - - - - - - 27 - 16777215 - - - - Delete Filter - - - - - - - 16 - 16 - - - - true - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui deleted file mode 100644 index 79db7845..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui +++ /dev/null @@ -1,56 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 47 - - - - Form - - - - 0 - - - - - - - - 16777215 - 45 - - - - Click an Item to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui deleted file mode 100644 index f9c2a2f9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui +++ /dev/null @@ -1,41 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - - - - Toggle Regular Expressions - - - Regex - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui deleted file mode 100644 index 9b3fb67a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui +++ /dev/null @@ -1,74 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - Start Time - - - hh:mm:ss.zzz (yyyy-MM-dd) - - - - - - - Disable End Time - - - - - - true - - - - - - - End Time - - - hh:mm:ss.zzz (yyyy-MM-dd) - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui deleted file mode 100644 index 5f463253..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui +++ /dev/null @@ -1,67 +0,0 @@ - - - TextBrowseDialog - - - - 0 - 0 - 585 - 562 - - - - Message Viewer - - - - - - - - - Qt::Horizontal - - - QDialogButtonBox::Close - - - - - - - - - button_box - accepted() - TextBrowseDialog - accept() - - - 248 - 254 - - - 157 - 274 - - - - - button_box - rejected() - TextBrowseDialog - reject() - - - 316 - 260 - - - 286 - 274 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console b/workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console deleted file mode 100755 index 47ce376e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_console.console.Console')) diff --git a/workspace/src/rqt_common_plugins/rqt_console/setup.py b/workspace/src/rqt_common_plugins/rqt_console/setup.py deleted file mode 100644 index c9f19532..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_console', 'rqt_console.filters'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_console'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/__init__.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py deleted file mode 100644 index 4104f58a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py +++ /dev/null @@ -1,139 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rosgraph_msgs.msg import Log -import rospkg -import rospy - -from python_qt_binding.QtCore import QMutex, QMutexLocker, QTimer - -from qt_gui.plugin import Plugin - -from .console_settings_dialog import ConsoleSettingsDialog -from .console_widget import ConsoleWidget -from .message import Message -from .message_data_model import MessageDataModel -from .message_proxy_model import MessageProxyModel - - -class Console(Plugin): - """ - rqt_console plugin's main class. Handles communication with ros_gui and contains - callbacks to handle incoming message - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(Console, self).__init__(context) - self.setObjectName('Console') - - self._rospack = rospkg.RosPack() - - self._model = MessageDataModel() - self._proxy_model = MessageProxyModel() - self._proxy_model.setSourceModel(self._model) - - self._widget = ConsoleWidget(self._proxy_model, self._rospack) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - # queue to store incoming data which get flushed periodically to the model - # required since QSortProxyModel can not handle a high insert rate - self._message_queue = [] - self._mutex = QMutex() - self._timer = QTimer() - self._timer.timeout.connect(self.insert_messages) - self._timer.start(100) - - self._subscriber = None - self._topic = '/rosout_agg' - self._subscribe(self._topic) - - def queue_message(self, log_msg): - """ - Callback for adding an incomming message to the queue. - """ - if not self._widget._paused: - msg = Console.convert_rosgraph_log_message(log_msg) - with QMutexLocker(self._mutex): - self._message_queue.append(msg) - - @staticmethod - def convert_rosgraph_log_message(log_msg): - msg = Message() - msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)') - msg.message = log_msg.msg - msg.severity = log_msg.level - msg.node = log_msg.name - msg.stamp = (log_msg.header.stamp.secs, log_msg.header.stamp.nsecs) - msg.topics = sorted(log_msg.topics) - msg.location = log_msg.file + ':' + log_msg.function + ':' + str(log_msg.line) - return msg - - def insert_messages(self): - """ - Callback for flushing incoming Log messages from the queue to the model. - """ - with QMutexLocker(self._mutex): - msgs = self._message_queue - self._message_queue = [] - if msgs: - self._model.insert_rows(msgs) - - def shutdown_plugin(self): - self._subscriber.unregister() - self._timer.stop() - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - - def trigger_configuration(self): - topics = [t for t in rospy.get_published_topics() if t[1] == 'rosgraph_msgs/Log'] - topics.sort(key=lambda tup: tup[0]) - dialog = ConsoleSettingsDialog(topics, self._rospack) - (topic, message_limit) = dialog.query(self._topic, self._model.get_message_limit()) - if topic != self._topic: - self._subscribe(topic) - if message_limit != self._model.get_message_limit(): - self._model.set_message_limit(message_limit) - - def _subscribe(self, topic): - if self._subscriber: - self._subscriber.unregister() - self._subscriber = rospy.Subscriber(topic, Log, self.queue_message) - self._currenttopic = topic diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py deleted file mode 100644 index 14c72394..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py +++ /dev/null @@ -1,73 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QDialog - -from rqt_logger_level.logger_level_widget import LoggerLevelWidget -from rqt_logger_level.logger_level_service_caller import LoggerLevelServiceCaller - - -class ConsoleSettingsDialog(QDialog): - """ - Dialog to change the subscribed topic and alter the message buffer size. - """ - def __init__(self, topics, rospack): - """ - :param topics: list of topics to allow switching, ''list of string'' - :param limit: displayed in the message buffer size spin box, ''int'' - """ - super(ConsoleSettingsDialog, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource', 'console_settings_dialog.ui') - loadUi(ui_file, self) - for topic in topics: - self.topic_combo.addItem(topic[0] + ' (' + topic[1] + ')', topic[0]) - - self._service_caller = LoggerLevelServiceCaller() - self._logger_widget = LoggerLevelWidget(self._service_caller) - self.levelsLayout.addWidget(self._logger_widget) - self.adjustSize() - - def query(self, topic, buffer_size): - index = self.topic_combo.findData(topic) - if index != -1: - self.topic_combo.setCurrentIndex(index) - self.buffer_size_spin.setValue(buffer_size) - ok = self.exec_() - if ok == 1: - index = self.topic_combo.currentIndex() - if index != -1: - topic = self.topic_combo.itemData(index) - buffer_size = self.buffer_size_spin.value() - return topic, buffer_size diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py deleted file mode 100644 index f96ceb73..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py +++ /dev/null @@ -1,754 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QApplication, QCursor, QFileDialog, QHeaderView,QIcon, QMenu, QMessageBox, QTableView, QWidget -from python_qt_binding.QtCore import QRegExp, Qt, qWarning - -import time -import datetime - -from rqt_py_common.ini_helper import pack, unpack - -from .filters.custom_filter import CustomFilter -from .filters.location_filter import LocationFilter -from .filters.message_filter import MessageFilter -from .filters.node_filter import NodeFilter -from .filters.severity_filter import SeverityFilter -from .filters.time_filter import TimeFilter -from .filters.topic_filter import TopicFilter - -from .filters.custom_filter_widget import CustomFilterWidget -from .filters.filter_wrapper_widget import FilterWrapperWidget -from .filters.list_filter_widget import ListFilterWidget -from .filters.text_filter_widget import TextFilterWidget -from .filters.time_filter_widget import TimeFilterWidget - -from .message import Message -from .message_data_model import MessageDataModel - -from .text_browse_dialog import TextBrowseDialog - - -class ConsoleWidget(QWidget): - """ - Primary widget for the rqt_console plugin. - """ - def __init__(self, proxy_model, rospack, minimal=False): - """ - :param proxymodel: the proxy model to display in the widget,''QSortFilterProxyModel'' - :param minimal: if true the load, save and column buttons will be hidden as well as the filter splitter, ''bool'' - """ - super(ConsoleWidget, self).__init__() - self._proxy_model = proxy_model - self._model = self._proxy_model.sourceModel() - self._paused = False - self._rospack = rospack - - # These are lists of Tuples = (,) - self._exclude_filters = [] - self._highlight_filters = [] - - ui_file = os.path.join(self._rospack.get_path('rqt_console'), 'resource', 'console_widget.ui') - loadUi(ui_file, self) - - if minimal: - self.load_button.hide() - self.save_button.hide() - self.column_resize_button.hide() - self.setObjectName('ConsoleWidget') - self.table_view.setModel(proxy_model) - - self._columnwidth = (60, 100, 70, 100, 100, 100, 100) - for idx, width in enumerate(self._columnwidth): - self.table_view.horizontalHeader().resizeSection(idx, width) - - def update_sort_indicator(logical_index, order): - if logical_index == 0: - self._proxy_model.sort(-1) - self.table_view.horizontalHeader().setSortIndicatorShown(logical_index != 0) - self.table_view.horizontalHeader().sortIndicatorChanged.connect(update_sort_indicator) - - self.add_exclude_button.setIcon(QIcon.fromTheme('list-add')) - self.add_highlight_button.setIcon(QIcon.fromTheme('list-add')) - self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) - if not self.pause_button.icon().isNull(): - self.pause_button.setText('') - self.record_button.setIcon(QIcon.fromTheme('media-record')) - if not self.record_button.icon().isNull(): - self.record_button.setText('') - self.load_button.setIcon(QIcon.fromTheme('document-open')) - if not self.load_button.icon().isNull(): - self.load_button.setText('') - self.save_button.setIcon(QIcon.fromTheme('document-save')) - if not self.save_button.icon().isNull(): - self.save_button.setText('') - self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) - if not self.clear_button.icon().isNull(): - self.clear_button.setText('') - self.highlight_exclude_button.setIcon(QIcon.fromTheme('format-text-strikethrough')) - - self.pause_button.clicked[bool].connect(self._handle_pause_clicked) - self.record_button.clicked[bool].connect(self._handle_record_clicked) - self.load_button.clicked[bool].connect(self._handle_load_clicked) - self.save_button.clicked[bool].connect(self._handle_save_clicked) - self.column_resize_button.clicked[bool].connect(self._handle_column_resize_clicked) - self.clear_button.clicked[bool].connect(self._handle_clear_button_clicked) - - self.table_view.mouseDoubleClickEvent = self._handle_mouse_double_click - self.table_view.mousePressEvent = self._handle_mouse_press - self.table_view.keyPressEvent = self._handle_custom_keypress - - self.highlight_exclude_button.clicked[bool].connect(self._proxy_model.set_show_highlighted_only) - - self.add_highlight_button.clicked.connect(self._add_highlight_filter) - self.add_exclude_button.clicked.connect(self._add_exclude_filter) - - # Filter factory dictionary: - # index 0 is a label describing the widget, index 1 is the class that provides filtering logic - # index 2 is the widget that sets the data in the filter class, index 3 are the arguments for the widget class constructor - self._filter_factory_order = ['message', 'severity', 'node', 'time', 'topic', 'location', 'custom'] - self.filter_factory = {'message': (self.tr('...containing'), MessageFilter, TextFilterWidget), - 'severity': (self.tr('...with severities'), SeverityFilter, ListFilterWidget, self._model.get_severity_dict), - 'node': (self.tr('...from node'), NodeFilter, ListFilterWidget, self._model.get_unique_nodes), - 'time': (self.tr('...from time range'), TimeFilter, TimeFilterWidget, self.get_time_range_from_selection), - 'topic': (self.tr('...from topic'), TopicFilter, ListFilterWidget, self._model.get_unique_topics), - 'location': (self.tr('...from location'), LocationFilter, TextFilterWidget), - 'custom': (self.tr('Custom'), CustomFilter, CustomFilterWidget, [self._model.get_severity_dict, self._model.get_unique_nodes, self._model.get_unique_topics])} - - self._model.rowsInserted.connect(self.update_status) - self._model.rowsRemoved.connect(self.update_status) - self._proxy_model.rowsInserted.connect(self.update_status) - self._proxy_model.rowsRemoved.connect(self.update_status) - - # list of TextBrowserDialogs to close when cleaning up - self._browsers = [] - - # This defaults the filters panel to start by taking 50% of the available space - if minimal: - self.table_splitter.setSizes([1, 0]) - else: - self.table_splitter.setSizes([1, 1]) - self.exclude_table.resizeColumnsToContents() - self.highlight_table.resizeColumnsToContents() - - def get_message_summary(self, start_time_offset=None, end_time_offset=None): - """ - :param start_time: number of seconds before now to start, ''int'' (optional) - :param end_time: number of seconds before now to end, ''int'' (optional) - :returns: summary of message numbers within time - """ - current_time = time.mktime(datetime.datetime.now().timetuple()) - if start_time_offset is None: - start_time = current_time - 240 - else: - start_time = current_time - start_time_offset - if end_time_offset is not None: - end_time = current_time - end_time_offset - else: - end_time = None - - message_subset = self._model.get_message_between(start_time, end_time) - - class Message_Summary(object): - __slots__ = 'fatal', 'error', 'warn', 'info', 'debug' - - def __init__(self, messages): - self.fatal = 0 - self.error = 0 - self.warn = 0 - self.info = 0 - self.debug = 0 - for message in messages: - if message.severity == Message.DEBUG: - self.debug += 1 - elif message.severity == Message.INFO: - self.info += 1 - elif message.severity == Message.WARN: - self.warn += 1 - elif message.severity == Message.ERROR: - self.error += 1 - elif message.severity == Message.FATAL: - self.fatal += 1 - else: - assert False, "Unknown severity type '%s'" % str(message.severity) - - return Message_Summary(message_subset) - - def get_time_range_from_selection(self): - """ - :returns: the range of time of messages in the current table selection (min, max), ''tuple(str,str)'' - """ - rowlist = [] - indexes = self.table_view.selectionModel().selectedIndexes() - - if indexes: - rowlist = [self._proxy_model.mapToSource(current).row() for current in indexes] - rowlist = sorted(list(set(rowlist))) - - mintime, maxtime = self._model.get_time_range(rowlist) - return (mintime, maxtime) - return (-1, -1) - - def _delete_highlight_filter(self): - """ - Deletes any highlight filters which have a checked delete button - """ - for index, item in enumerate(self._highlight_filters): - if item[1].delete_button.isChecked(): - self._proxy_model.delete_highlight_filter(index) - self.highlight_table.removeCellWidget(index, 0) - self.highlight_table.removeRow(index) - item[0].filter_changed_signal.disconnect(self._proxy_model.handle_highlight_filters_changed) - item[1].delete_button.clicked.disconnect(self._delete_highlight_filter) - del self._highlight_filters[index] - - def _delete_exclude_filter(self): - """ - Deletes any exclude filters which have a checked delete button - """ - for index, item in enumerate(self._exclude_filters): - if item[1].delete_button.isChecked(): - self._proxy_model.delete_exclude_filter(index) - self.exclude_table.removeCellWidget(index, 0) - self.exclude_table.removeRow(index) - item[0].filter_changed_signal.disconnect(self._proxy_model.handle_exclude_filters_changed) - item[1].delete_button.clicked.disconnect(self._delete_exclude_filter) - del self._exclude_filters[index] - - def _add_highlight_filter(self, filter_index=False): - """ - :param filter_index: if false then this function shows a QMenu to allow the user to choose a type of message filter. ''bool'' - OR - :param filter_index: the index of the filter to be added, ''int'' - :return: if a filter was added then the index is returned, ''int'' - OR - :return: if no filter was added then None is returned, ''NoneType'' - """ - if filter_index is False: - filter_index = -1 - filter_select_menu = QMenu() - for index in self._filter_factory_order: - # flattens the _highlight filters list and only adds the item if it doesn't already exist - if index in ['message', 'location'] or not self.filter_factory[index][1] in [type(item) for sublist in self._highlight_filters for item in sublist]: - filter_select_menu.addAction(self.filter_factory[index][0]) - action = filter_select_menu.exec_(QCursor.pos()) - if action is None: - return - for index in self._filter_factory_order: - if self.filter_factory[index][0] == action.text(): - filter_index = index - if filter_index == -1: - return - - index = len(self._highlight_filters) - newfilter = self.filter_factory[filter_index][1]() - if len(self.filter_factory[filter_index]) >= 4: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack, self.filter_factory[filter_index][3]) - else: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack) - - # pack the new filter tuple onto the filter list - self._highlight_filters.append((newfilter, FilterWrapperWidget(newwidget, self.filter_factory[filter_index][0]), filter_index)) - self._proxy_model.add_highlight_filter(newfilter) - newfilter.filter_changed_signal.connect(self._proxy_model.handle_highlight_filters_changed) - self._highlight_filters[index][1].delete_button.clicked.connect(self._delete_highlight_filter) - self._model.rowsInserted.connect(self._highlight_filters[index][1].repopulate) - - # place the widget in the proper location - self.highlight_table.insertRow(index) - self.highlight_table.setCellWidget(index, 0, self._highlight_filters[index][1]) - self.highlight_table.resizeColumnsToContents() - self.highlight_table.resizeRowsToContents() - newfilter.filter_changed_signal.emit() - return index - - def _add_exclude_filter(self, filter_index=False): - """ - :param filter_index: if false then this function shows a QMenu to allow the user to choose a type of message filter. ''bool'' - OR - :param filter_index: the index of the filter to be added, ''int'' - :return: if a filter was added then the index is returned, ''int'' - OR - :return: if no filter was added then None is returned, ''NoneType'' - """ - if filter_index is False: - filter_index = -1 - filter_select_menu = QMenu() - for index in self._filter_factory_order: - # flattens the _exclude filters list and only adds the item if it doesn't already exist - if index in ['message', 'location'] or not self.filter_factory[index][1] in [type(item) for sublist in self._exclude_filters for item in sublist]: - filter_select_menu.addAction(self.filter_factory[index][0]) - action = filter_select_menu.exec_(QCursor.pos()) - if action is None: - return None - for index in self._filter_factory_order: - if self.filter_factory[index][0] == action.text(): - filter_index = index - if filter_index == -1: - return None - - index = len(self._exclude_filters) - newfilter = self.filter_factory[filter_index][1]() - if len(self.filter_factory[filter_index]) >= 4: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack, self.filter_factory[filter_index][3]) - else: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack) - - # pack the new filter tuple onto the filter list - self._exclude_filters.append((newfilter, FilterWrapperWidget(newwidget, self.filter_factory[filter_index][0]), filter_index)) - self._proxy_model.add_exclude_filter(newfilter) - newfilter.filter_changed_signal.connect(self._proxy_model.handle_exclude_filters_changed) - self._exclude_filters[index][1].delete_button.clicked.connect(self._delete_exclude_filter) - self._model.rowsInserted.connect(self._exclude_filters[index][1].repopulate) - - # place the widget in the proper location - self.exclude_table.insertRow(index) - self.exclude_table.setCellWidget(index, 0, self._exclude_filters[index][1]) - self.exclude_table.resizeColumnsToContents() - self.exclude_table.resizeRowsToContents() - newfilter.filter_changed_signal.emit() - return index - - def _process_highlight_exclude_filter(self, selection, selectiontype, exclude=False): - """ - Modifies the relevant filters (based on selectiontype) to remove (exclude=True) - or highlight (exclude=False) the selection from the dataset in the tableview. - :param selection: the actual selection, ''str'' - :param selectiontype: the type of selection, ''str'' - :param exclude: If True process as an exclude filter, False process as an highlight filter, ''bool'' - """ - types = {self.tr('Node'): 2, self.tr('Topic'): 4, self.tr('Severity'): 1, self.tr('Message'): 0} - try: - col = types[selectiontype] - except: - raise RuntimeError("Bad Column name in ConsoleWidget._process_highlight_exclude_filter()") - - if col == 0: - unique_messages = set() - selected_indexes = self.table_view.selectionModel().selectedIndexes() - num_selected = len(selected_indexes) / 6 - for index in range(num_selected): - unique_messages.add(selected_indexes[num_selected * col + index].data()) - unique_messages = list(unique_messages) - for message in unique_messages: - message = message.replace('\\', '\\\\') - message = message.replace('.', '\\.') - if exclude: - filter_index = self._add_exclude_filter(selectiontype.lower()) - filter_widget = self._exclude_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - else: - filter_index = self._add_highlight_filter(col) - filter_widget = self._highlight_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - filter_widget.set_regex(True) - filter_widget.set_text('^' + message + '$') - - else: - if exclude: - # Test if the filter we are adding already exists if it does use the existing filter - if self.filter_factory[selectiontype.lower()][1] not in [type(item) for sublist in self._exclude_filters for item in sublist]: - filter_index = self._add_exclude_filter(selectiontype.lower()) - else: - for index, item in enumerate(self._exclude_filters): - if type(item[0]) == self.filter_factory[selectiontype.lower()][1]: - filter_index = index - else: - # Test if the filter we are adding already exists if it does use the existing filter - if self.filter_factory[selectiontype.lower()][1] not in [type(item) for sublist in self._highlight_filters for item in sublist]: - filter_index = self._add_highlight_filter(col) - else: - for index, item in enumerate(self._highlight_filters): - if type(item[0]) == self.filter_factory[selectiontype.lower()][1]: - filter_index = index - - if exclude: - filter_widget = self._exclude_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - filter_widget.select_item(selection) - else: - filter_widget = self._highlight_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - filter_widget.select_item(selection) - - def _rightclick_menu(self, event): - """ - Dynamically builds the rightclick menu based on the unique column data - from the passed in datamodel and then launches it modally - :param event: the mouse event object, ''QMouseEvent'' - """ - severities = {} - for severity, label in Message.SEVERITY_LABELS.items(): - if severity in self._model.get_unique_severities(): - severities[severity] = label - nodes = sorted(self._model.get_unique_nodes()) - topics = sorted(self._model.get_unique_topics()) - - # menutext entries turned into - menutext = [] - menutext.append([self.tr('Exclude'), [[self.tr('Severity'), severities], [self.tr('Node'), nodes], [self.tr('Topic'), topics], [self.tr('Selected Message(s)')]]]) - menutext.append([self.tr('Highlight'), [[self.tr('Severity'), severities], [self.tr('Node'), nodes], [self.tr('Topic'), topics], [self.tr('Selected Message(s)')]]]) - menutext.append([self.tr('Copy Selected')]) - menutext.append([self.tr('Browse Selected')]) - - menu = QMenu() - submenus = [] - subsubmenus = [] - for item in menutext: - if len(item) > 1: - submenus.append(QMenu(item[0], menu)) - for subitem in item[1]: - if len(subitem) > 1: - subsubmenus.append(QMenu(subitem[0], submenus[-1])) - if isinstance(subitem[1], dict): - for key in sorted(subitem[1].keys()): - action = subsubmenus[-1].addAction(subitem[1][key]) - action.setData(key) - else: - for subsubitem in subitem[1]: - subsubmenus[-1].addAction(subsubitem) - submenus[-1].addMenu(subsubmenus[-1]) - else: - submenus[-1].addAction(subitem[0]) - menu.addMenu(submenus[-1]) - else: - menu.addAction(item[0]) - action = menu.exec_(event.globalPos()) - - if action is None or action == 0: - return - elif action.text() == self.tr('Browse Selected'): - self._show_browsers() - elif action.text() == self.tr('Copy Selected'): - rowlist = [] - for current in self.table_view.selectionModel().selectedIndexes(): - rowlist.append(self._proxy_model.mapToSource(current).row()) - copytext = self._model.get_selected_text(rowlist) - if copytext is not None: - clipboard = QApplication.clipboard() - clipboard.setText(copytext) - elif action.text() == self.tr('Selected Message(s)'): - if action.parentWidget().title() == self.tr('Highlight'): - self._process_highlight_exclude_filter(action.text(), 'Message', False) - elif action.parentWidget().title() == self.tr('Exclude'): - self._process_highlight_exclude_filter(action.text(), 'Message', True) - else: - raise RuntimeError("Menu format corruption in ConsoleWidget._rightclick_menu()") - else: - # This processes the dynamic list entries (severity, node and topic) - try: - roottitle = action.parentWidget().parentWidget().title() - except: - raise RuntimeError("Menu format corruption in ConsoleWidget._rightclick_menu()") - - if roottitle == self.tr('Highlight'): - self._process_highlight_exclude_filter(action.text(), action.parentWidget().title(), False) - elif roottitle == self.tr('Exclude'): - self._process_highlight_exclude_filter(action.text(), action.parentWidget().title(), True) - else: - raise RuntimeError("Unknown Root Action %s selected in ConsoleWidget._rightclick_menu()" % roottitle) - - def update_status(self): - """ - Sets the message display label to the current value - """ - if self._model.rowCount() == self._proxy_model.rowCount(): - tip = self.tr('Displaying %d messages') % (self._model.rowCount()) - else: - tip = self.tr('Displaying %d of %d messages') % (self._proxy_model.rowCount(), self._model.rowCount()) - self.messages_label.setText(tip) - - def cleanup_browsers_on_close(self): - for browser in self._browsers: - browser.close() - - def _show_browsers(self): - rowlist = [] - for current in self.table_view.selectionModel().selectedIndexes(): - rowlist.append(self._proxy_model.mapToSource(current).row()) - browsetext = self._model.get_selected_text(rowlist) - if browsetext is not None: - self._browsers.append(TextBrowseDialog(browsetext, self._rospack)) - self._browsers[-1].show() - - def _handle_clear_button_clicked(self, checked): - self._model.remove_rows([]) - Message._next_id = 1 - - def _handle_load_clicked(self, checked): - filename = QFileDialog.getOpenFileName(self, self.tr('Load from File'), '.', self.tr('rqt_console message file {.csv} (*.csv)')) - if filename[0] != '': - try: - with open(filename[0], 'r') as h: - lines = h.read().splitlines() - except IOError as e: - qWarning(str(e)) - return False - - # extract column header - columns = lines[0].split(';') - if len(lines) < 2: - return True - - # join wrapped lines - rows = [] - last_wrapped = False - for line in lines[1:]: - # ignore empty lines - if not line: - continue - # check for quotes and remove them - if line == '"': - has_prefix = not last_wrapped - has_suffix = last_wrapped - line = '' - else: - has_prefix = line[0] == '"' - if has_prefix: - line = line[1:] - has_suffix = line[-1] == '"' - if has_suffix: - line = line[:-1] - - # ignore line without prefix if previous line was not wrapped - if not has_prefix and not last_wrapped: - continue - # remove wrapped line which is not continued on the next line - if last_wrapped and has_prefix: - rows.pop() - - # add/append lines - if last_wrapped: - rows[-1] += line - else: - # add line without quote prefix - rows.append(line) - - last_wrapped = not has_suffix - - # generate message for each row - messages = [] - skipped = [] - for row in rows: - data = row.split('";"') - msg = Message() - msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)') - for i, column in enumerate(columns): - value = data[i] - if column == 'message': - msg.message = value.replace('\\"', '"') - elif column == 'severity': - msg.severity = int(value) - if msg.severity not in Message.SEVERITY_LABELS: - skipped.append('Unknown severity value: %s' % value) - msg = None - break - elif column == 'stamp': - parts = value.split('.') - if len(parts) != 2: - skipped.append('Unknown timestamp format: %s' % value) - msg = None - break - msg.stamp = (int(parts[0]), int(parts[1])) - elif column == 'topics': - msg.topics = value.split(',') - elif column == 'node': - msg.node = value - elif column == 'location': - msg.location = value - else: - skipped.append('Unknown column: %s' % column) - msg = None - break - if msg: - messages.append(msg) - if skipped: - qWarning('Skipped %d rows since they do not appear to be in rqt_console message file format:\n- %s' % (len(skipped), '\n- '.join(skipped))) - - if messages: - self._model.insert_rows(messages) - - self._handle_pause_clicked(True) - - return True - - else: - qWarning('File does not appear to be a rqt_console message file: missing file header.') - return False - - def _handle_save_clicked(self, checked): - filename = QFileDialog.getSaveFileName(self, 'Save to File', '.', self.tr('rqt_console msg file {.csv} (*.csv)')) - if filename[0] != '': - filename = filename[0] - if filename[-4:] != '.csv': - filename += '.csv' - try: - handle = open(filename, 'w') - except IOError as e: - qWarning(str(e)) - return - try: - handle.write(';'.join(MessageDataModel.columns) + '\n') - for index in range(self._proxy_model.rowCount()): - row = self._proxy_model.mapToSource(self._proxy_model.index(index, 0)).row() - msg = self._model._messages[row] - data = {} - data['message'] = msg.message.replace('"', '\\"') - data['severity'] = str(msg.severity) - data['node'] = msg.node - data['stamp'] = str(msg.stamp[0]) + '.' + str(msg.stamp[1]).zfill(9) - data['topics'] = ','.join(msg.topics) - data['location'] = msg.location - line = [] - for column in MessageDataModel.columns: - line.append('"%s"' % data[column]) - handle.write(';'.join(line) + '\n') - except Exception as e: - qWarning('File save failed: %s' % str(e)) - return False - finally: - handle.close() - return True - - def _handle_pause_clicked(self): - self._paused = True - self.pause_button.setVisible(False) - self.record_button.setVisible(True) - - def _handle_record_clicked(self): - self._paused = False - self.pause_button.setVisible(True) - self.record_button.setVisible(False) - - def _handle_column_resize_clicked(self): - self.table_view.resizeColumnsToContents() - - def _delete_selected_rows(self): - rowlist = [] - for current in self.table_view.selectionModel().selectedIndexes(): - rowlist.append(self._proxy_model.mapToSource(current).row()) - rowlist = list(set(rowlist)) - return self._model.remove_rows(rowlist) - - def _handle_custom_keypress(self, event, old_keyPressEvent=QTableView.keyPressEvent): - """ - Handles the delete key. - The delete key removes the tableview's selected rows from the datamodel - """ - if event.key() == Qt.Key_Delete and len(self._model._messages) > 0: - delete = QMessageBox.Yes - if len(self.table_view.selectionModel().selectedIndexes()) == 0: - delete = QMessageBox.question(self, self.tr('Message'), self.tr("Are you sure you want to delete all messages?"), QMessageBox.Yes | QMessageBox.No, QMessageBox.No) - if delete == QMessageBox.Yes and event.key() == Qt.Key_Delete and event.modifiers() == Qt.NoModifier: - if self._delete_selected_rows(): - event.accept() - return old_keyPressEvent(self.table_view, event) - - def _handle_mouse_double_click(self, event, old_doubleclickevent=QTableView.mouseDoubleClickEvent): - if event.buttons() & Qt.LeftButton and event.modifiers() == Qt.NoModifier: - self._show_browsers() - event.accept() - return old_doubleclickevent(self.table_view, event) - - def _handle_mouse_press(self, event, old_pressEvent=QTableView.mousePressEvent): - if event.buttons() & Qt.RightButton and event.modifiers() == Qt.NoModifier: - self._rightclick_menu(event) - event.accept() - return old_pressEvent(self.table_view, event) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('settings_exist', True) - - instance_settings.set_value('table_splitter', self.table_splitter.saveState()) - instance_settings.set_value('filter_splitter', self.filter_splitter.saveState()) - - instance_settings.set_value('paused', self._paused) - instance_settings.set_value('show_highlighted_only', self.highlight_exclude_button.isChecked()) - - exclude_filters = [] - for index, item in enumerate(self._exclude_filters): - exclude_filters.append(item[2]) - filter_settings = instance_settings.get_settings('exclude_filter_' + str(index)) - item[1].save_settings(filter_settings) - instance_settings.set_value('exclude_filters', pack(exclude_filters)) - - highlight_filters = [] - for index, item in enumerate(self._highlight_filters): - highlight_filters.append(item[2]) - filter_settings = instance_settings.get_settings('highlight_filter_' + str(index)) - item[1].save_settings(filter_settings) - instance_settings.set_value('highlight_filters', pack(highlight_filters)) - instance_settings.set_value('message_limit', self._model.get_message_limit()) - - def restore_settings(self, pluggin_settings, instance_settings): - if instance_settings.contains('table_splitter'): - self.table_splitter.restoreState(instance_settings.value('table_splitter')) - if instance_settings.contains('filter_splitter'): - self.filter_splitter.restoreState(instance_settings.value('filter_splitter')) - else: - self.filter_splitter.setSizes([1, 1]) - - paused = instance_settings.value('paused') in [True, 'true'] - if paused: - self._handle_pause_clicked() - else: - self._handle_record_clicked() - self.highlight_exclude_button.setChecked(instance_settings.value('show_highlighted_only') in [True, 'true']) - self._proxy_model.set_show_highlighted_only(self.highlight_exclude_button.isChecked()) - - for item in self._exclude_filters: - item[1].delete_button.setChecked(True) - self._delete_exclude_filter() - if instance_settings.contains('exclude_filters'): - exclude_filters = unpack(instance_settings.value('exclude_filters')) - if exclude_filters is not None: - for index, item in enumerate(exclude_filters): - self._add_exclude_filter(item) - filter_settings = instance_settings.get_settings('exclude_filter_' + str(index)) - self._exclude_filters[-1][1].restore_settings(filter_settings) - else: - self._add_exclude_filter('severity') - - for item in self._highlight_filters: - item[1].delete_button.setChecked(True) - self._delete_highlight_filter() - if instance_settings.contains('highlight_filters'): - highlight_filters = unpack(instance_settings.value('highlight_filters')) - if highlight_filters is not None: - for index, item in enumerate(highlight_filters): - self._add_highlight_filter(item) - filter_settings = instance_settings.get_settings('highlight_filter_' + str(index)) - self._highlight_filters[-1][1].restore_settings(filter_settings) - else: - self._add_highlight_filter('message') - - if instance_settings.contains('message_limit'): - self._model.set_message_limit(int(instance_settings.value('message_limit'))) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/__init__.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py deleted file mode 100644 index 8dda1006..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py +++ /dev/null @@ -1,77 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QObject, QTimer, Signal - - -class BaseFilter(QObject): - """ - Contains basic functions common to all filters. - Handles enabled logic and change notification. - """ - filter_changed_signal = Signal() - - def __init__(self): - super(BaseFilter, self).__init__() - self._enabled = True - - self._timer = QTimer(self) - self._timer.setSingleShot(True) - self._timer.timeout.connect(self.filter_changed_signal.emit) - - def start_emit_timer(self, msec=None): - """ - Starts a timer to emit a signal to refresh the filters after the filter is changed - :param msec: number of msecs to wait before emitting the signal to change the filter ''int'' - """ - if msec is None: - self._timer.start(10) - else: - self._timer.start(msec) - - def is_enabled(self): - return self._enabled - - def set_enabled(self, checked): - """ - Setter for _enabled - :param checked: boolean flag to set ''bool'' - :emits filter_changed_signal: Always - """ - self._enabled = checked - self.start_emit_timer(200) - - def has_filter(self): - raise NotImplementedError() - - def test_message(self, message): - raise NotImplementedError() diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py deleted file mode 100644 index 2e42e473..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py +++ /dev/null @@ -1,93 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter -from .message_filter import MessageFilter -from .node_filter import NodeFilter -from .severity_filter import SeverityFilter -from .topic_filter import TopicFilter - - -class CustomFilter(BaseFilter): - """ - Contains filter logic for the custom filter which allows message, severity, - node and topic filtering simultaniously. All of these filters must match - together (if they are used) or the custom filter does not match. - """ - - def __init__(self): - super(CustomFilter, self).__init__() - - self._message = MessageFilter() - self._severity = SeverityFilter() - self._node = NodeFilter() - self._topic = TopicFilter() - - self._all_filters = [self._message, self._severity, self._node, self._topic] - for f in self._all_filters: - f.filter_changed_signal.connect(self._relay_signal) - - def set_enabled(self, checked): - """ - :signal: emits filter_changed_signal - :param checked: enables the filters if checked is True''bool'' - """ - for f in self._all_filters: - f.set_enabled(checked) - super(CustomFilter, self).set_enabled(checked) - - def _relay_signal(self): - """ - Passes any signals emitted by the child filters along - """ - self.start_emit_timer(1) - - def has_filter(self): - for f in self._all_filters: - if f.has_filter(): - return True - return False - - def test_message(self, message): - """ - Tests if the message matches the filter. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches all child filters, ''bool'' - """ - if not self.is_enabled(): - return False - # if non of the subfilters contains any input the custom filter does not match - if not self.has_filter(): - return False - # the custom filter matches when all subfilters which contain input match - all_filters = [not f.has_filter() or f.test_message(message) for f in self._all_filters] - return False not in all_filters diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py deleted file mode 100644 index 22b6d57b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py +++ /dev/null @@ -1,180 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QPalette, QWidget - -from rqt_py_common.ini_helper import pack, unpack - - -class CustomFilterWidget(QWidget): - def __init__(self, parentfilter, rospack, item_providers): - super(CustomFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'custom_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('CustomFilterWidget') - self._parentfilter = parentfilter # When data is changed it is stored in the parent filter - - # keep color for highlighted items even when not active - for list_widget in [self.severity_list, self.node_list, self.topic_list]: - active_color = list_widget.palette().brush(QPalette.Active, QPalette.Highlight).color().name() - list_widget.setStyleSheet('QListWidget:item:selected:!active { background: %s; }' % active_color) - - # Text Filter Initialization - self.text_edit.textChanged.connect(self.handle_text_changed) - self.regex_check_box.clicked[bool].connect(self.handle_regex_clicked) - self.handle_text_changed() - - # Severity Filter Initialization - self.severity_list.itemSelectionChanged.connect(self.handle_severity_item_changed) - new_items = item_providers[0]() - for key in sorted(new_items.keys()): - item = new_items[key] - self.severity_list.addItem(item) - self.severity_list.item(self.severity_list.count() - 1).setData(Qt.UserRole, key) - - # Node Filter Initialization - self._node_list_populate_function = item_providers[1] - self.node_list.itemSelectionChanged.connect(self.handle_node_item_changed) - - # Topic Filter Initialization - self._topic_list_populate_function = item_providers[2] - self.topic_list.itemSelectionChanged.connect(self.handle_topic_item_changed) - - self.repopulate() - - def handle_node_item_changed(self): - self._parentfilter._node.set_selected_items(self.node_list.selectedItems()) - - def handle_topic_item_changed(self): - self._parentfilter._topic.set_selected_items(self.topic_list.selectedItems()) - - def handle_severity_item_changed(self): - self._parentfilter._severity.set_selected_items(self.severity_list.selectedItems()) - - def handle_text_changed(self): - self._parentfilter._message.set_text(self.text_edit.text()) - - def handle_regex_clicked(self, clicked): - self._parentfilter._message.set_regex(clicked) - - def repopulate(self): - """ - Repopulates the display widgets based on the function arguments passed - in during initialization - """ - newset = self._topic_list_populate_function() - for item in newset: - if len(self.topic_list.findItems(item, Qt.MatchExactly)) == 0: - self._add_item(self.topic_list, item) - - newset = self._node_list_populate_function() - for item in newset: - if len(self.node_list.findItems(item, Qt.MatchExactly)) == 0: - self._add_item(self.node_list, item) - - def _add_item(self, list_widget, item): - """ - Insert item in alphabetical order. - """ - for i in range(list_widget.count()): - if item <= list_widget.item(i).text(): - list_widget.insertItem(i, item) - return - list_widget.addItem(item) - - def save_settings(self, settings): - """ - Saves the settings for this filter to an ini file. - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('text', self._parentfilter._message._text) - settings.set_value('regex', self._parentfilter._message._regex) - - settings.set_value('severityitemlist', pack(self._parentfilter._severity._selected_items)) - - settings.set_value('topicitemlist', pack(self._parentfilter._topic._selected_items)) - - settings.set_value('nodeitemlist', pack(self._parentfilter._node._selected_items)) - - return - - def restore_settings(self, settings): - """ - Restores the settings for this filter from an ini file. - :param settings: used to extract the settings from an ini file ''qt_gui.settings.Settings'' - """ - text = settings.value('text', '') - self.text_edit.setText(text) - self.handle_text_changed() - - regex = settings.value('regex') in [True, 'true'] - self.regex_check_box.setChecked(regex) - self.handle_regex_clicked(regex) - - #Restore Severities - for index in range(self.severity_list.count()): - self.severity_list.item(index).setSelected(False) - severity_item_list = unpack(settings.value('severityitemlist')) - for item in severity_item_list: - items = self.severity_list.findItems(item, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_severity_item_changed() - - #Restore Topics - for index in range(self.topic_list.count()): - self.topic_list.item(index).setSelected(False) - topic_item_list = unpack(settings.value('topicitemlist')) - for item in topic_item_list: - if len(self.topic_list.findItems(item, Qt.MatchExactly)) == 0: - self.topic_list.addItem(item) - items = self.topic_list.findItems(item, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_topic_item_changed() - - #Restore Nodes - for index in range(self.node_list.count()): - self.node_list.item(index).setSelected(False) - node_item_list = unpack(settings.value('nodeitemlist')) - for item in node_item_list: - if len(self.node_list.findItems(item, Qt.MatchExactly)) == 0: - self.node_list.addItem(item) - items = self.node_list.findItems(item, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_node_item_changed() diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py deleted file mode 100644 index 841be238..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py +++ /dev/null @@ -1,68 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -class FilterCollection: - """ - Collection of filters to test messages against. - """ - def __init__(self): - self._filters = [] - - def test_message(self, message, default=False): - """ - Test if the message matches any filter. - :param message: message to be tested against the filters, ''Message'' - :param default: return value when there is no active filter, ''bool'' - :returns: True if the message matches any filter, ''bool'' - """ - match = default - for f in self._filters: - if f.is_enabled() and f.has_filter(): - if f.test_message(message): - return True - else: - match = False - return match - - def append(self, filter): - self._filters.append(filter) - - def count_enabled_filters(self): - enabled = [f for f in self._filters if f.is_enabled()] - return len(enabled) - - def __len__(self): - return len(self._filters) - - def __delitem__(self, index): - del self._filters[index] diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py deleted file mode 100644 index 5d7a1025..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py +++ /dev/null @@ -1,94 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QIcon, QWidget - - -class FilterWrapperWidget(QWidget): - """ - Wraps the other filter widgets to provide enable check box, delete button control and uniform labeling - """ - def __init__(self, widget, filter_name): - """ - :param widget: the Qwidget to wrap ''Qwidget'' - :param filter_name: the name to be placed on the label ''str'' - """ - super(FilterWrapperWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_console'), 'resource/filters', 'filter_wrapper_widget.ui') - loadUi(ui_file, self) - self.setObjectName('FilterWrapperWidget') - self.delete_button.setIcon(QIcon.fromTheme('list-remove')) - self._widget = widget - - # Replace the placeholder QWidget with the passed in object - stretch = self.layout_frame.stretch(2) - self.layout_frame.insertWidget(2, widget) - self.layout_frame.setStretch(2, stretch) - - # This line sets the place holder to 0 width so it can no longer be seen - # It is a hack since removing it causes other widgets not to function properly - self.layout_frame.setStretch(3, 0) - - self.enabled_checkbox.clicked[bool].connect(self.enabled_callback) - self.filter_name_label.setText(filter_name + ':') - - def enabled_callback(self, checked): - self._widget._parentfilter.set_enabled(checked) - self._widget.setEnabled(checked) - - def repopulate(self): - self._widget.repopulate() - - def save_settings(self, settings): - """ - Handles writing the enabled flag to the ini file and then passes the - settings object to the wrapped widget - - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('enabled', self._widget._parentfilter._enabled) - self._widget.save_settings(settings) - - def restore_settings(self, settings): - """ - Handles reading the enabled flag from the ini file. - :param settings: used to read the settings to an ini file ''qt_gui.settings.Settings'' - """ - checked = settings.value('enabled') in [True, 'true'] - self.enabled_callback(checked) - self.enabled_checkbox.setChecked(checked) - self._widget.restore_settings(settings) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py deleted file mode 100644 index 46c9ff3a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py +++ /dev/null @@ -1,133 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QPalette, QWidget - -from rqt_py_common.ini_helper import pack, unpack - - -class ListFilterWidget(QWidget): - """ - Generic List widget to be used when implementing filters that require - limited dynamic selections - """ - def __init__(self, parentfilter, rospack, item_provider): - """ - :param parentfilter: The filter object, must implement set_list and - contain _list ''QObject'' - :param item_provider: a function designed to provide a list or dict - """ - super(ListFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'list_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('ListFilterWidget') - self._parentfilter = parentfilter # When data is changed we need to store it in the parent filter - - # keep color for highlighted items even when not active - active_color = self.palette().brush(QPalette.Active, QPalette.Highlight).color().name() - self.setStyleSheet('QListWidget:item:selected:!active { background: %s; }' % active_color) - - self._list_populate_function = item_provider - self.list_widget.itemSelectionChanged.connect(self.handle_item_changed) - self._display_list = [] - self.repopulate() - - def select_item(self, text): - """ - All items matching text will be selected in the list_widget - :param item: a string to be matched against the list ''str'' - """ - items = self.list_widget.findItems(text, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_item_changed() - - def handle_item_changed(self): - self._parentfilter.set_selected_items(self.list_widget.selectedItems()) - - def repopulate(self): - """ - Repopulates the display widgets based on the function arguments passed - in during initialization - """ - new_items = self._list_populate_function() - - new_set = set(new_items.values() if isinstance(new_items, dict) else new_items) - - if len(new_items) != len(self._display_list): - if isinstance(new_items, dict): - # for dictionaries store the key as user role data - for key in sorted(new_items.keys()): - item = new_items[key] - if item not in self._display_list: - self.list_widget.addItem(item) - self.list_widget.item(self.list_widget.count() - 1).setData(Qt.UserRole, key) - else: - for item in new_items: - if item not in self._display_list: - self._add_item(item) - self._display_list = sorted(set(new_set) | set(self._display_list)) - - def _add_item(self, item): - """ - Insert item in alphabetical order. - """ - for i in range(self.list_widget.count()): - if item <= self.list_widget.item(i).text(): - self.list_widget.insertItem(i, item) - return - self.list_widget.addItem(item) - - def save_settings(self, settings): - """ - Saves the settings for this filter. - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('itemlist', pack(self._parentfilter._selected_items)) - - def restore_settings(self, settings): - """ - Restores the settings for this filter from an ini file. - :param settings: used to extract the settings from an ini file ''qt_gui.settings.Settings'' - """ - for index in range(self.list_widget.count()): - self.list_widget.item(index).setSelected(False) - item_list = unpack(settings.value('itemlist')) - for item in item_list: - if len(self.list_widget.findItems(item, Qt.MatchExactly)) == 0: - self.list_widget.addItem(item) - self.select_item(item) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py deleted file mode 100644 index 7d3acb5f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py +++ /dev/null @@ -1,45 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .message_filter import MessageFilter - - -class LocationFilter(MessageFilter): - """ - Contains filter logic for a location filter. - """ - - def __init__(self): - super(LocationFilter, self).__init__() - - def test_message(self, message): - return self._test_message(message.location) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py deleted file mode 100644 index 21513910..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py +++ /dev/null @@ -1,104 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QRegExp -from .base_filter import BaseFilter - - -class MessageFilter(BaseFilter): - """ - Contains filter logic for a message filter. If the regex flag is False - simple 'is this in that' text matching is used on _text. If the regex flag is True - _text is treated as a regular expression with one exception. If it does not - start with a ^ a .* is appended, and if it does not end with a $ then a .* - is added to the end. - The filter_changed signal should be connected to a slot which notifies the - overall filtering system that it needs to reevaluate all entries. - """ - - def __init__(self): - super(MessageFilter, self).__init__() - self._text = '' - self._regex = False - - def set_text(self, text): - """ - Setter for _text - :param text: text to set ''str'' - :emits filter_changed_signal: If _enabled is true - """ - self._text = text - if self.is_enabled(): - self.start_emit_timer(500) - - def set_regex(self, checked): - """ - Setter for _regex - :param checked: boolean flag to set ''bool'' - :emits filter_changed_signal: If _enabled is true - """ - self._regex = checked - if self.is_enabled(): - self.start_emit_timer(500) - - def has_filter(self): - return self._text != '' - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the regex flag is False simple 'is this in that' text matching is used - on _text. If the regex flag is True _text is treated as a regular expression - with one exception. If it does not start with a ^ a .* is appended, and if - it does not end with a $ then a .* is added to the end. - - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - return self._test_message(message.message) - - def _test_message(self, value): - if not self.is_enabled(): - return False - if self._text != '': - if self._regex: - temp = self._text - if temp[0] != '^': - temp = '.*' + temp - if temp[-1] != '$': - temp += '.*' - if QRegExp(temp).exactMatch(value): - return True - else: - if value.find(self._text) != -1: - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py deleted file mode 100644 index 7787674f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py +++ /dev/null @@ -1,72 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter - - -class NodeFilter(BaseFilter): - """ - Contains filter logic for a single node filter. - If the message's node text matches any of the text in the stored list - then it is considered a match. - """ - def __init__(self): - super(NodeFilter, self).__init__() - self._selected_items = [] - - def set_selected_items(self, items): - """ - Setter for selected items. - :param list_" list of items to store for filtering ''list of QListWidgetItem'' - :emits filter_changed_signal: If _enabled is true - """ - self._selected_items = items - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return len(self._selected_items) > 0 - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the message's node text matches any of the text in the stored list - then it is considered a match. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - for item in self._selected_items: - if message.node == item.text(): - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py deleted file mode 100644 index a4bd2530..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py +++ /dev/null @@ -1,74 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter - -from python_qt_binding.QtCore import Qt - - -class SeverityFilter(BaseFilter): - """ - Contains filter logic for a severity filter. - If the message's severity text matches any of the text in the stored list - then it is considered a match. - """ - def __init__(self): - super(SeverityFilter, self).__init__() - self._selected_items = [] - - def set_selected_items(self, items): - """ - Setter for selected items. - :param list_: list of items to store for filtering ''list of QListWidgetItem'' - :emits filter_changed_signal: If _enabled is true - """ - self._selected_items = items - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return len(self._selected_items) > 0 - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the message's severity text matches any of the text in the stored list - then it is considered a match. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - for item in self._selected_items: - if message.severity == item.data(Qt.UserRole): - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py deleted file mode 100644 index a8b0373b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py +++ /dev/null @@ -1,97 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget - - -class TextFilterWidget(QWidget): - def __init__(self, parentfilter, rospack): - """ - Widget for displaying interactive data related to text filtering. - :param parentfilter: buddy filter were data is stored, ''TimeFilter'' - """ - super(TextFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'text_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('TextFilterWidget') - self._parentfilter = parentfilter # When data is changed it is stored in the parent filter - - self.text_edit.textChanged.connect(self.handle_text_changed) - self.regex_check_box.clicked[bool].connect(self.handle_regex_clicked) - - self.handle_text_changed() - - def set_text(self, text): - """ - Setter for the text edit widget - :param text: text to be placed in text_edit, ''str'' - """ - self.text_edit.setText(text) - - def set_regex(self, checked): - """ - Setter for the text edit widget - :param text: text to be placed in text_edit, ''str'' - """ - self.regex_check_box.setChecked(checked) - self.handle_regex_clicked(checked) - - def handle_text_changed(self): - self._parentfilter.set_text(self.text_edit.text()) - - def handle_regex_clicked(self, clicked): - self._parentfilter.set_regex(clicked) - - def repopulate(self): - """ - Stub function. - If the widget had any dynamically adjustable data it would requery it - in this function. - """ - pass - - def save_settings(self, settings): - settings.set_value('text', self._parentfilter._text) - settings.set_value('regex', self._parentfilter._regex) - - def restore_settings(self, settings): - text = settings.value('text', '') - self.set_text(text) - self.handle_text_changed() - - regex = settings.value('regex') in [True, 'true'] - self.set_regex(regex) - self.handle_regex_clicked(regex) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py deleted file mode 100644 index 37fc08bb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py +++ /dev/null @@ -1,100 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QDateTime -from .base_filter import BaseFilter - - -class TimeFilter(BaseFilter): - """ - Contains filter logic for a time filter. - If _stop_time_enabled is true then the message's time value must be between the dates provided - to be considered a match - If _stop_time_enabled is false then the time must simply be after _start_time - """ - - def __init__(self): - super(TimeFilter, self).__init__() - self._start_time = QDateTime() - self._stop_time = QDateTime() - self._stop_time_enabled = True - - def set_start_time(self, time): - """ - Setter for _start_time - :param time" start datetime for filter ''QDateTime'' - :emits filter_changed_signal: If _enabled is true - """ - self._start_time = time - if self.is_enabled(): - self.start_emit_timer() - - def set_stop_time(self, time): - """ - Setter for _stop_time - :param time" stop datetime for filter ''QDateTime'' - :emits filter_changed_signal: If _enabled is true - """ - self._stop_time = time - if self.is_enabled(): - self.start_emit_timer() - - def set_stop_time_enabled(self, checked): - """ - Setter for _stop_time_enabled - :param checked" boolean flag to set ''bool'' - :emits filter_changed_signal: If _enabled is true - """ - self._stop_time_enabled = checked - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return True - - def test_message(self, message): - """ - Tests if the message matches the filter. - If _stop_time_enabled is true then the message's time value must be between the dates provided - to be considered a match - If _stop_time_enabled is false then the time must simply be after _start_time - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - message_time = message.get_stamp_as_qdatetime() - if message_time < self._start_time: - return False - if self._stop_time_enabled and self._stop_time < message_time: - return False - return True diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py deleted file mode 100644 index 858e4a3d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py +++ /dev/null @@ -1,121 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from datetime import datetime -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QDateTime -from python_qt_binding.QtGui import QWidget - - -class TimeFilterWidget(QWidget): - def __init__(self, parentfilter, rospack, time_range_provider): - """ - Widget for displaying interactive data related to time filtering. - :param parentfilter: buddy filter were data is stored, ''TimeFilter'' - :param display_list_args: single element list containing one tuple with - the min and max time to be displayed, ''list of tuple'' - """ - super(TimeFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'time_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('TimeFilterWidget') - self._parentfilter = parentfilter # When data is changed it is stored in the parent filter - - self.start_datetime.dateTimeChanged[QDateTime].connect(self.handle_start_changed) - self.stop_datetime.dateTimeChanged[QDateTime].connect(self.handle_stop_changed) - self.stop_enabled_check_box.clicked[bool].connect(self.handle_stop_enabled_changed) - - # Times are passed in unixtimestamp '.' decimal fraction of a second - mintime, maxtime = time_range_provider() - if mintime != -1: - mintime = str(mintime).split('.') - maxtime = str(maxtime).split('.') - - time = QDateTime() - time.setTime_t(int(mintime[0])) - mintime = time.addMSecs(int(str(mintime[1]).zfill(9)[:3])) - self.start_datetime.setDateTime(mintime) - time.setTime_t(int(maxtime[0])) - maxtime = time.addMSecs(int(str(maxtime[1]).zfill(9)[:3])) - self.stop_datetime.setDateTime(maxtime) - else: - self.start_datetime.setDateTime(datetime.now()) - self.stop_datetime.setDateTime(datetime.now()) - - def handle_start_changed(self, datetime_): - self._parentfilter.set_start_time(datetime_) - - def handle_stop_changed(self, datetime_): - self._parentfilter.set_stop_time(datetime_) - - def handle_stop_enabled_changed(self, checked): - self._parentfilter.set_stop_time_enabled(checked) - self.stop_datetime.setEnabled(checked) - - def repopulate(self): - """ - Stub function. - If the widget had any dynamically adjustable data it would requery it - in this function. - """ - pass - - def save_settings(self, settings): - """ - Saves the settings for this filter to an ini file. - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('start_time', self._parentfilter._start_time.toString('hh:mm:ss.zzz (yyyy-MM-dd)')) - settings.set_value('stop_time', self._parentfilter._stop_time.toString('hh:mm:ss.zzz (yyyy-MM-dd)')) - settings.set_value('stop_time_enabled', self._parentfilter._stop_time_enabled) - - def restore_settings(self, settings): - """ - Restores the settings for this filter from an ini file. - :param settings: used to extract the settings from an ini file ''qt_gui.settings.Settings'' - """ - self.handle_stop_enabled_changed(settings.value('stop_time_enabled') in [True, 'true']) - if settings.contains('start_time'): - self.handle_start_changed(QDateTime.fromString(settings.value('start_time'), 'hh:mm:ss.zzz (yyyy-MM-dd)')) - else: - self.handle_start_changed(QDateTime(datetime.now())) - if settings.contains('stop_time'): - self.handle_stop_changed(QDateTime.fromString(settings.value('stop_time'), 'hh:mm:ss.zzz (yyyy-MM-dd)')) - else: - self.handle_stop_changed(QDateTime(datetime.now())) - - self.stop_datetime.setDateTime(self._parentfilter._stop_time) - self.start_datetime.setDateTime(self._parentfilter._start_time) - self.stop_enabled_check_box.setChecked(self._parentfilter._stop_time_enabled) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py deleted file mode 100644 index 6a06a2c4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py +++ /dev/null @@ -1,72 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter - - -class TopicFilter(BaseFilter): - """ - Contains filter logic for a topic filter. - If the message's topic text matches any of the text in the stored list - then it is considered a match. - """ - def __init__(self): - super(TopicFilter, self).__init__() - self._selected_items = [] - - def set_selected_items(self, items): - """ - Setter for selected items. - :param list_" list of items to store for filtering ''list of QListWidgetItem'' - :emits filter_changed_signal: If _enabled is true - """ - self._selected_items = items - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return len(self._selected_items) > 0 - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the message's topic text matches any of the text in the stored list - then it is considered a match. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - for item in self._selected_items: - if item.text() in message.topics: - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py deleted file mode 100644 index 901ccfd4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py +++ /dev/null @@ -1,133 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rosgraph_msgs.msg import Log - -from python_qt_binding.QtCore import QCoreApplication, QDateTime, QObject - - -class Message(QObject): - - DEBUG = 1 - INFO = 2 - WARN = 4 - ERROR = 8 - FATAL = 16 - - SEVERITY_LABELS = { - DEBUG: QCoreApplication.translate('Message', 'Debug'), - INFO: QCoreApplication.translate('Message', 'Info'), - WARN: QCoreApplication.translate('Message', 'Warn'), - ERROR: QCoreApplication.translate('Message', 'Error'), - FATAL: QCoreApplication.translate('Message', 'Fatal'), - } - - _next_id = 1 - - def __init__(self): - super(Message, self).__init__() - self.id = Message._next_id - Message._next_id += 1 - - self.message = None - self.severity = None - self.node = None - self.__stamp = (None, None) - self.topics = [] - self.location = None - - self._stamp_compare = None - self._stamp_qdatetime = None - - self._stamp_format = None - self._stamp_string = None - - self.highlighted = True - - def _get_stamp(self): - return self.__stamp - - def _set_stamp(self, stamp): - """ - Update the string representation of the timestamp. - :param stamp: a tuple containing seconds and nano seconds - """ - assert len(stamp) == 2 - self.__stamp = stamp - if None not in self.__stamp: - # shortest string representation to compare stamps - # floats do not provide enough precision - self._stamp_compare = '%08x%08x' % (self.__stamp[0], self.__stamp[1]) - else: - self._stamp_compare = None - self._stamp_qdatetime = self._get_stamp_as_qdatetime(self.__stamp) - if self._stamp_format: - s = self._stamp_qdatetime.toString(self._stamp_format) - if 'ZZZ' in s: - s = s.replace('ZZZ', str(self.__stamp[1]).zfill(9)) - self._stamp_string = s - - stamp = property(_get_stamp, _set_stamp) - - def get_stamp_for_compare(self): - return self._stamp_compare - - def get_stamp_as_qdatetime(self): - return self._stamp_qdatetime - - def _get_stamp_as_qdatetime(self, stamp): - if None in self.__stamp: - return None - dt = QDateTime() - dt.setTime_t(stamp[0]) - dt.addMSecs(int(float(stamp[1]) / 10**6)) - return dt - - def get_stamp_string(self): - return self._stamp_string - - def set_stamp_format(self, format): - self._stamp_format = format - if None not in self.__stamp: - self.stamp = self.__stamp - - def pretty_print(self): - text = self.tr('Node: ') + self.node + '\n' - text += self.tr('Time: ') + self.get_stamp_string() + '\n' - text += self.tr('Severity: ') + Message.SEVERITY_LABELS[self.severity] + '\n' - text += self.tr('Published Topics: ') + ', '.join(self.topics) + '\n' - text += '\n' + self.message + '\n' - text += '\n' + 'Location:' - text += '\n' + self.location + '\n\n' - text += '-' * 100 + '\n\n' - - return text diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py deleted file mode 100644 index 7b32eab3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py +++ /dev/null @@ -1,267 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QAbstractTableModel, QModelIndex, Qt, qWarning -from python_qt_binding.QtGui import QBrush, QIcon - -from .message import Message -from .message_list import MessageList - - -class MessageDataModel(QAbstractTableModel): - - # the column names must match the message attributes - columns = ['message', 'severity', 'node', 'stamp', 'topics', 'location'] - - severity_colors = { - Message.DEBUG: QBrush(Qt.darkCyan), - Message.INFO: QBrush(Qt.darkBlue), - Message.WARN: QBrush(Qt.darkYellow), - Message.ERROR: QBrush(Qt.darkRed), - Message.FATAL: QBrush(Qt.red), - } - - def __init__(self): - super(MessageDataModel, self).__init__() - self._messages = MessageList() - self._message_limit = 20000 - self._info_icon = QIcon.fromTheme('dialog-information') - self._warning_icon = QIcon.fromTheme('dialog-warning') - self._error_icon = QIcon.fromTheme('dialog-error') - - # BEGIN Required implementations of QAbstractTableModel functions - - def rowCount(self, parent=None): - return len(self._messages) - - def columnCount(self, parent=None): - return len(MessageDataModel.columns) + 1 - - def data(self, index, role=None): - if role is None: - role = Qt.DisplayRole - if index.row() >= 0 and index.row() < len(self._messages): - msg = self._messages[index.row()] - if index.column() == 0: - if role == Qt.DisplayRole: - return '#%d' % msg.id - elif index.column() > 0 and index.column() < len(MessageDataModel.columns) + 1: - column = MessageDataModel.columns[index.column() - 1] - if role == Qt.DisplayRole or role == Qt.UserRole: - if column == 'stamp': - if role != Qt.UserRole: - data = msg.get_stamp_string() - else: - data = msg.get_stamp_for_compare() - else: - data = getattr(msg, column) - # map severity enum to label - if role == Qt.DisplayRole and column == 'severity': - data = Message.SEVERITY_LABELS[data] - # implode topic names - if column == 'topics': - data = ', '.join(data) - # append row number to define strict order - if role == Qt.UserRole: - # append row number to define strict order - # shortest string representation to compare stamps - #print(column, data, str(index.row()).zfill(len(str(len(self._messages))))) - data = str(data) + ' %08x' % index.row() - return data - - # decorate message column with severity icon - if role == Qt.DecorationRole and column == 'message': - if msg.severity in [Message.DEBUG, Message.INFO]: - return self._info_icon - elif msg.severity in [Message.WARN]: - return self._warning_icon - elif msg.severity in [Message.ERROR, Message.FATAL]: - return self._error_icon - - # colorize severity label - if role == Qt.ForegroundRole and column =='severity': - assert msg.severity in MessageDataModel.severity_colors, 'Unknown severity type: %s' % msg.severity - return MessageDataModel.severity_colors[msg.severity] - - if role == Qt.ToolTipRole and column != 'severity': - if column == 'stamp': - data = msg.get_stamp_string() - elif column == 'topics': - data = ', '.join(msg.topics) - else: - data = getattr(msg, column) - # tag enables word wrap by forcing rich text - return '' + data + '

' + self.tr('Right click for menu.') + '' - - def headerData(self, section, orientation, role=None): - if role is None: - role = Qt.DisplayRole - if orientation == Qt.Horizontal: - if role == Qt.DisplayRole: - if section == 0: - return self.tr('#') - else: - return MessageDataModel.columns[section - 1].capitalize() - if role == Qt.ToolTipRole: - if section == 0: - return self.tr('Sort the rows by serial number in descendig order') - else: - return self.tr('Sorting the table by a column other then the serial number slows down the interaction especially when recording high frequency data') - - # END Required implementations of QAbstractTableModel functions - - def get_message_limit(self): - return self._message_limit - - def set_message_limit(self, new_limit): - self._message_limit = new_limit - self._enforce_message_limit(self._message_limit) - - def _enforce_message_limit(self, limit): - if len(self._messages) > limit: - self.beginRemoveRows(QModelIndex(), limit, len(self._messages) - 1) - del self._messages[limit:len(self._messages)] - self.endRemoveRows() - - def insert_rows(self, msgs): - # never try to insert more message than the limit - if len(msgs) > self._message_limit: - msgs = msgs[-self._message_limit:] - # reduce model before insert - limit = self._message_limit - len(msgs) - self._enforce_message_limit(limit) - # insert newest messages - self.beginInsertRows(QModelIndex(), 0, len(msgs) - 1) - self._messages.extend(msgs) - self.endInsertRows() - - def remove_rows(self, rowlist): - """ - :param rowlist: list of row indexes, ''list(int)'' - :returns: True if the indexes were removed successfully, ''bool'' - """ - if len(rowlist) == 0: - if len(self._messages) > 0: - try: - self.beginRemoveRows(QModelIndex(), 0, len(self._messages)) - del self._messages[0:len(self._messages)] - self.endRemoveRows() - except: - return False - else: - rowlist = list(set(rowlist)) - rowlist.sort(reverse=True) - dellist = [rowlist[0]] - for row in rowlist[1:]: - if dellist[-1] - 1 > row: - try: - self.beginRemoveRows(QModelIndex(), dellist[-1], dellist[0]) - del self._messages[dellist[-1]:dellist[0] + 1] - self.endRemoveRows() - except: - return False - dellist = [] - dellist.append(row) - if len(dellist) > 0: - try: - self.beginRemoveRows(QModelIndex(), dellist[-1], dellist[0]) - del self._messages[dellist[-1]:dellist[0] + 1] - self.endRemoveRows() - except: - return False - return True - - def get_selected_text(self, rowlist): - """ - Returns an easily readable block of text for the currently selected rows - :param rowlist: list of row indexes, ''list(int)'' - :returns: the text from those indexes, ''str'' - """ - text = None - if len(rowlist) != 0: - text = '' - rowlist = list(set(rowlist)) - for row in rowlist: - text += self._messages[row].pretty_print() - return text - - def get_time_range(self, rowlist): - """ - :param rowlist: a list of row indexes, ''list'' - :returns: a tuple of min and max times in a rowlist in '(unix timestamp).(fraction of second)' format, ''tuple(str,str)'' - """ - min_ = float("inf") - max_ = float("-inf") - for row in rowlist: - item = self._messages[row].time_as_datestamp() - if float(item) > float(max_): - max_ = item - if float(item) < float(min_): - min_ = item - return min_, max_ - - def get_unique_nodes(self): - nodes = set([]) - for message in self._messages: - nodes.add(message.node) - return nodes - - def get_unique_severities(self): - severities = set([]) - for message in self._messages: - severities.add(message.severity) - return severities - - def get_unique_topics(self): - topics = set([]) - for message in self._messages: - for topic in message.topics: - topics.add(topic) - return topics - - def get_severity_dict(self): - return Message.SEVERITY_LABELS - - def get_message_between(self, start_time, end_time=None): - """ - :param start_time: time to start in timestamp form (including decimal - fractions of a second is acceptable, ''unixtimestamp'' - :param end_time: time to end in timestamp form (including decimal - fractions of a second is acceptable, ''unixtimestamp'' (Optional) - :returns: list of messages in the time range ''list[message]'' - """ - msgs = [] - for message in self._messages: - msg_time = message.stamp[0] + float(message.stamp[1]) / 10**9 - if msg_time >= float(start_time) and (end_time is None or msg_time <= float(end_time)): - msgs.append(message) - return msgs diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py deleted file mode 100644 index b0c1c910..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py +++ /dev/null @@ -1,63 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Open Source Robotics Foundation Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -class MessageList(object): - - def __init__(self): - super(MessageList, self).__init__() - self._messages = [] - - def __getitem__(self, key): - return self._messages[len(self._messages) - key - 1] - - def __delitem__(self, key): - if isinstance(key, slice): - assert key.step is None or key.step == 1, 'MessageList.__delitem__ not implemented for slices with step argument different than 1' - del self._messages[len(self._messages) - key.stop:len(self._messages) - key.start] - else: - del self._messages[len(self._messages) - key - 1] - - def __iter__(self): - return reversed(self._messages) - - def __reversed__(self): - return iter(self._messages) - - def __contains__(self, item): - return item in self._messages - - def __len__(self): - return len(self._messages) - - def extend(self, item): - self._messages.extend(item) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py deleted file mode 100644 index 62554733..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py +++ /dev/null @@ -1,132 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Qt, qWarning -from python_qt_binding.QtGui import QBrush, QColor, QSortFilterProxyModel - -from .filters.filter_collection import FilterCollection -from .message import Message - - -class MessageProxyModel(QSortFilterProxyModel): - """ - Provides sorting and filtering capabilities for the MessageDataModel. - Filtering is based on a collection of exclude and highlight filters. - """ - - def __init__(self): - super(MessageProxyModel, self).__init__() - self.setDynamicSortFilter(True) - self.setFilterRole(Qt.UserRole) - self.setSortCaseSensitivity(Qt.CaseInsensitive) - self.setSortRole(Qt.UserRole) - - self._exclude_filters = FilterCollection() - self._highlight_filters = FilterCollection() - self._show_highlighted_only = False - - # caching source model locally for better performance of filterAcceptsRow() - self._source_model = None - - def setSourceModel(self, source_model): - super(MessageProxyModel, self).setSourceModel(source_model) - self._source_model = self.sourceModel() - - # BEGIN Required implementations of QSortFilterProxyModel functions - - def filterAcceptsRow(self, sourcerow, sourceparent): - """ - returns: True if the row does not match any exclude filter AND (_show_highlighted_only is False OR it matches any highlight filter), ''bool'' - """ - msg = self._source_model._messages[sourcerow] - if self._exclude_filters.test_message(msg): - # hide excluded message - return False - - highlighted = True - if self._highlight_filters.count_enabled_filters() > 0: - highlighted = self._highlight_filters.test_message(msg, default=True) - if self._show_highlighted_only and not highlighted: - # hide messages which are not highlighted when only highlightes messages should be visible - return False - - # update message state - msg.highlighted = highlighted - - return True - - def data(self, proxy_index, role=None): - """ - Set colors of items based on highlight filters. - """ - index = self.mapToSource(proxy_index) - if role == Qt.ForegroundRole: - msg = self._source_model._messages[index.row()] - if not msg.highlighted: - return QBrush(Qt.gray) - return self._source_model.data(index, role) - - # END Required implementations of QSortFilterProxyModel functions - - def handle_exclude_filters_changed(self): - """ - Invalidate filters and trigger refiltering. - """ - self.invalidateFilter() - - def handle_highlight_filters_changed(self): - """ - Invalidate filters and trigger refiltering. - """ - if self._show_highlighted_only: - self.invalidateFilter() - else: - self.invalidateFilter() - self.dataChanged.emit(self.index(0, 0), self.index(self.rowCount() - 1, self.columnCount() - 1)) - - def add_exclude_filter(self, newfilter): - self._exclude_filters.append(newfilter) - - def add_highlight_filter(self, newfilter): - self._highlight_filters.append(newfilter) - - def delete_exclude_filter(self, index): - del self._exclude_filters[index] - self.handle_exclude_filters_changed() - - def delete_highlight_filter(self, index): - del self._highlight_filters[index] - self.handle_highlight_filters_changed() - - def set_show_highlighted_only(self, show_highlighted_only): - self._show_highlighted_only = show_highlighted_only - self.invalidateFilter() diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py deleted file mode 100644 index 3449aac7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py +++ /dev/null @@ -1,50 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QDialog - - -class TextBrowseDialog(QDialog): - """ - Simple text brower Dialog that sets its text from the passed in text. - """ - def __init__(self, text, rospack): - """ - :param text: value to set the text of the widget to, ''str'' - """ - super(TextBrowseDialog, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource', 'text_browse_dialog.ui') - loadUi(ui_file, self) - self.text_browser.setText(text) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst deleted file mode 100644 index dacde9f1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst +++ /dev/null @@ -1,111 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_dep -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* install rqt_dep globally (`#286 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* using CATKIN_ENABLE_TESTING to optionally configure tests (`#220 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* make it a non ros plugin startable without a roscore (`#200 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* add partial support for metapackages -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- -* fix handling ResourceNotFound for not found packages in graph - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* add combo to filter wet/dry packages - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- -* improve graph generation, get rid of redundant traversal, remove max edge limit - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into Groovy diff --git a/workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt deleted file mode 100644 index 0da391bc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_dep) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test/dotcode_pack_test.py) -endif() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_dep - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox deleted file mode 100644 index bba00f3f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_dep provides a GUI plugin for visualizing the ROS dependency graph. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_dep/package.xml b/workspace/src/rqt_common_plugins/rqt_dep/package.xml deleted file mode 100644 index 925469e1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_dep - 0.3.13 - rqt_dep provides a GUI plugin for visualizing the ROS dependency graph. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_dep - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Thibault Kruse - - catkin - - python-rospkg - qt_dotgraph - qt_gui - qt_gui_py_common - rqt_graph - rqt_gui_py - - python-mock - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_dep/plugin.xml b/workspace/src/rqt_common_plugins/rqt_dep/plugin.xml deleted file mode 100644 index 63e9ecf7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin to visualize a ROS package network. - - - - - folder - Plugins related to introspection. - - - preferences-system-network - A Python GUI plugin for visualizing the ROS package dependencies. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui b/workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui deleted file mode 100644 index d06c7164..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui +++ /dev/null @@ -1,274 +0,0 @@ - - - RosPackGraphWidget - - - true - - - - 0 - 0 - 1144 - 307 - - - - Package Graph - - - - - - - - - Refresh ROS graph - - - - 16 - 16 - - - - - - - - - 0 - 0 - - - - Choose graph type - - - - - - - Comma separated regex patterns / names, leading dash creates exclusion pattern. Example: gazebo, -test.* - - - (Separate pkgs by comma) - - - - - - - - 0 - 0 - - - - - - - - - 0 - 0 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Load DOT graph from file - - - - 16 - 16 - - - - - - - - Save as DOT graph - - - - 16 - 16 - - - - - - - - Save as SVG - - - - 16 - 16 - - - - - - - - Save as image - - - - 16 - 16 - - - - - - - - - - - - - Mark Selected - - - Mark - - - - - - - Show stacks - - - Show Stacks - - - - - - - Do not show transitive / duplicate dependencies - - - No Transitive deps - - - - - - - Random colors for stacks - - - Colorize - - - true - - - - - - - Qt::Vertical - - - - - - - Highlight incoming and outgoing connections on mouse over - - - Highlight - - - true - - - - - - - Automatically fit graph into view on update - - - Fit - - - true - - - - - - - Fit graph in view - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - QPainter::Antialiasing|QPainter::HighQualityAntialiasing|QPainter::SmoothPixmapTransform|QPainter::TextAntialiasing - - - QGraphicsView::AnchorViewCenter - - - - - - - - InteractiveGraphicsView - QGraphicsView -
rqt_graph.interactive_graphics_view
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep b/workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep deleted file mode 100755 index 961a30c0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_dep.ros_pack_graph.RosPackGraph')) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/setup.py b/workspace/src/rqt_common_plugins/rqt_dep/setup.py deleted file mode 100644 index 893ad5a6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_dep'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_dep'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/__init__.py b/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py b/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py deleted file mode 100644 index fae10652..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py +++ /dev/null @@ -1,396 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import with_statement, print_function - -import os -import re - -from rospkg import MANIFEST_FILE -from rospkg.common import ResourceNotFound -from qt_dotgraph.colors import get_color_for_string - - -def matches_any(name, patternlist): - for pattern in patternlist: - if name == pattern: - return True - if re.match("^[a-zA-Z0-9_]+$", pattern) is None: - if re.match(pattern, name) is not None: - return True - return False - - -class RosPackageGraphDotcodeGenerator: - - def __init__(self, rospack, rosstack): - """ - :param rospack: use rospkg.RosPack() - :param rosstack: use rospkg.RosStack() - """ - self.rospack = rospack - self.rosstack = rosstack - self.stacks = {} - self.packages = {} - self.package_types = {} - self.edges = {} - self.traversed_ancestors = {} - self.traversed_descendants = {} - self.last_drawargs = None - self.last_selection = None - - def generate_dotcode(self, - dotcode_factory, - selected_names=[], - excludes=[], - depth=3, - with_stacks=True, - descendants=True, - ancestors=True, - hide_transitives=True, - mark_selected=True, - colortheme=None, - rank='same', # None, same, min, max, source, sink - ranksep=0.2, # vertical distance between layers - rankdir='TB', # direction of layout (TB top > bottom, LR left > right) - simplify=True, # remove double edges - force_refresh=False, - hide_wet=False, - hide_dry=False): - """ - - :param hide_transitives: if true, then dependency of children to grandchildren will be hidden if parent has same dependency - """ - - # defaults - selected_names = filter(lambda x: x is not None and x != '', selected_names) - excludes = filter(lambda x: x is not None and x != '', excludes) - if selected_names is None or selected_names == []: - selected_names = ['.*'] - self.depth = 1 - if depth is None: - depth = -1 - - # update arguments - - selection_args = { - "dotcode_factory": dotcode_factory, - "with_stacks": with_stacks, - "depth": depth, - "hide_transitives": hide_transitives, - "selected_names": selected_names, - "excludes": excludes, - "ancestors": ancestors, - "descendants": descendants, - "hide_wet": hide_wet, - "hide_dry": hide_dry - } - - # if selection did not change, we need not build up the graph again - selection_changed = False - if self.last_selection != selection_args: - selection_changed = True - self.last_selection = selection_args - - self.dotcode_factory = dotcode_factory - self.with_stacks = with_stacks - self.depth = depth - self.hide_transitives = hide_transitives - self.selected_names = selected_names - self.excludes = excludes - self.ancestors = ancestors - self.descendants = descendants - self.hide_wet = hide_wet - self.hide_dry = hide_dry - - if force_refresh or selection_changed: - self.stacks = {} - self.packages = {} - self.package_types = {} - self.edges = {} - self.traversed_ancestors = {} - self.traversed_descendants = {} - # update internal graph structure - for name in self.rospack.list(): - if matches_any(name, self.selected_names): - if descendants: - self.add_package_descendants_recursively(name) - if ancestors: - self.add_package_ancestors_recursively(name) - for stackname in self.rosstack.list(): - if matches_any(stackname, self.selected_names): - manifest = self.rosstack.get_manifest(stackname) - if manifest.is_catkin: - if descendants: - self.add_package_descendants_recursively(stackname) - if ancestors: - self.add_package_ancestors_recursively(stackname) - else: - for package_name in self.rosstack.packages_of(stackname): - if descendants: - self.add_package_descendants_recursively(package_name) - if ancestors: - self.add_package_ancestors_recursively(package_name) - - drawing_args = { - 'dotcode_factory': dotcode_factory, - "rank": rank, - "rankdir": rankdir, - "ranksep": ranksep, - "simplify": simplify, - "colortheme": colortheme, - "mark_selected": mark_selected - } - - # if selection and display args did not change, no need to generate dotcode - display_changed = False - if self.last_drawargs != drawing_args: - display_changed = True - self.last_drawargs = drawing_args - - self.dotcode_factory = dotcode_factory - self.rank = rank - self.rankdir = rankdir - self.ranksep = ranksep - self.simplify = simplify - self.colortheme = colortheme - self.dotcode_factory = dotcode_factory - self.mark_selected = mark_selected - - #generate new dotcode - if force_refresh or selection_changed or display_changed: - self.graph = self.generate(self.dotcode_factory) - self.dotcode = dotcode_factory.create_dot(self.graph) - - return self.dotcode - - def generate(self, dotcode_factory): - graph = dotcode_factory.get_graph(rank=self.rank, - rankdir=self.rankdir, - ranksep=self.ranksep, - simplify=self.simplify) - # print("In generate", self.with_stacks, len(self.stacks), len(self.packages), len(self.edges)) - packages_in_stacks = [] - if self.with_stacks and not self.hide_dry: - for stackname in self.stacks: - color = None - if self.mark_selected and not '.*' in self.selected_names and matches_any(stackname, self.selected_names): - color = 'tomato' - else: - color = 'gray' - if self.colortheme is not None: - color = get_color_for_string(stackname) - g = dotcode_factory.add_subgraph_to_graph(graph, - stackname, - color=color, - rank=self.rank, - rankdir=self.rankdir, - ranksep=self.ranksep, - simplify=self.simplify) - - for package_name in self.stacks[stackname]['packages']: - packages_in_stacks.append(package_name) - self._generate_package(dotcode_factory, g, package_name) - - for package_name, attributes in self.packages.iteritems(): - if package_name not in packages_in_stacks: - self._generate_package(dotcode_factory, graph, package_name, attributes) - for name1, name2 in self.edges.keys(): - dotcode_factory.add_edge_to_graph(graph, name1, name2) - return graph - - def _generate_package(self, dotcode_factory, graph, package_name, attributes=None): - if self._hide_package(package_name): - return - color = None - if self.mark_selected and not '.*' in self.selected_names and matches_any(package_name, self.selected_names): - if attributes and attributes['is_catkin']: - color = 'red' - else: - color = 'tomato' - elif attributes and not attributes['is_catkin']: - color = 'gray' - if attributes and 'not_found' in attributes and attributes['not_found']: - color = 'orange' - package_name += ' ?' - dotcode_factory.add_node_to_graph(graph, package_name, color=color) - - def _add_stack(self, stackname): - if stackname is None or stackname in self.stacks: - return - self.stacks[stackname] = {'packages': []} - - def _add_package(self, package_name, parent=None): - """ - adds object based on package_name to self.packages - :param parent: packagename which referenced package_name (for debugging only) - """ - if self._hide_package(package_name): - return - if package_name in self.packages: - return False - - catkin_package = self._is_package_wet(package_name) - if catkin_package is None: - return False - self.packages[package_name] = {'is_catkin': catkin_package} - - if self.with_stacks: - try: - stackname = self.rospack.stack_of(package_name) - except ResourceNotFound as e: - print('RosPackageGraphDotcodeGenerator._add_package(%s), parent %s: ResourceNotFound:' % (package_name, parent), e) - stackname = None - if not stackname is None and stackname != '': - if not stackname in self.stacks: - self._add_stack(stackname) - self.stacks[stackname]['packages'].append(package_name) - return True - - def _hide_package(self, package_name): - if not self.hide_wet and not self.hide_dry: - return False - catkin_package = self._is_package_wet(package_name) - if self.hide_wet and catkin_package: - return True - if self.hide_dry and catkin_package is False: - return True - # if type of package is unknown don't hide it - return False - - def _is_package_wet(self, package_name): - if package_name not in self.package_types: - try: - package_path = self.rospack.get_path(package_name) - manifest_file = os.path.join(package_path, MANIFEST_FILE) - self.package_types[package_name] = not os.path.exists(manifest_file) - except ResourceNotFound: - return None - return self.package_types[package_name] - - def _add_edge(self, name1, name2, attributes=None): - if self._hide_package(name1) or self._hide_package(name2): - return - self.edges[(name1, name2)] = attributes - - def add_package_ancestors_recursively(self, package_name, expanded_up=None, depth=None, implicit=False, parent=None): - """ - :param package_name: the name of package for which to add ancestors - :param expanded_up: names that have already been expanded (to avoid cycles) - :param depth: how many layers to follow - :param implicit: arg to rospack - :param parent: package that referenced package_name for error message only - """ - if package_name in self.traversed_ancestors: - traversed_depth = self.traversed_ancestors[package_name] - if traversed_depth is None: - return - if depth is not None and traversed_depth >= depth: - return - self.traversed_ancestors[package_name] = depth - - if matches_any(package_name, self.excludes): - return False - if (depth == 0): - return False - if (depth == None): - depth = self.depth - self._add_package(package_name, parent=parent) - if expanded_up is None: - expanded_up = [] - expanded_up.append(package_name) - if (depth != 1): - try: - depends_on = self.rospack.get_depends_on(package_name, implicit=implicit) - except ResourceNotFound as e: - print('RosPackageGraphDotcodeGenerator.add_package_ancestors_recursively(%s), parent %s: ResourceNotFound:' % (package_name, parent), e) - depends_on = [] - new_nodes = [] - for dep_on_name in [x for x in depends_on if not matches_any(x, self.excludes)]: - if not self.hide_transitives or not dep_on_name in expanded_up: - new_nodes.append(dep_on_name) - self._add_edge(dep_on_name, package_name) - self._add_package(dep_on_name, parent=package_name) - expanded_up.append(dep_on_name) - for dep_on_name in new_nodes: - self.add_package_ancestors_recursively(package_name=dep_on_name, - expanded_up=expanded_up, - depth=depth - 1, - implicit=implicit, - parent=package_name) - - def add_package_descendants_recursively(self, package_name, expanded=None, depth=None, implicit=False, parent=None): - if package_name in self.traversed_descendants: - traversed_depth = self.traversed_descendants[package_name] - if traversed_depth is None: - return - if depth is not None and traversed_depth >= depth: - return - self.traversed_descendants[package_name] = depth - - if matches_any(package_name, self.excludes): - return - if (depth == 0): - return - if (depth == None): - depth = self.depth - self._add_package(package_name, parent=parent) - if expanded is None: - expanded = [] - expanded.append(package_name) - if (depth != 1): - try: - try: - depends = self.rospack.get_depends(package_name, implicit=implicit) - except ResourceNotFound: - # try falling back to rosstack to find wet metapackages - manifest = self.rosstack.get_manifest(package_name) - if manifest.is_catkin: - depends = [d.name for d in manifest.depends] - else: - raise - except ResourceNotFound as e: - print('RosPackageGraphDotcodeGenerator.add_package_descendants_recursively(%s), parent: %s: ResourceNotFound:' % (package_name, parent), e) - depends = [] - new_nodes = [] - for dep_name in [x for x in depends if not matches_any(x, self.excludes)]: - if not self.hide_transitives or not dep_name in expanded: - new_nodes.append(dep_name) - self._add_edge(package_name, dep_name) - self._add_package(dep_name, parent=package_name) - expanded.append(dep_name) - for dep_name in new_nodes: - self.add_package_descendants_recursively(package_name=dep_name, - expanded=expanded, - depth=depth - 1, - implicit=implicit, - parent=package_name) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py b/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py deleted file mode 100644 index f86d424d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py +++ /dev/null @@ -1,417 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2011, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os -import pickle - -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QFile, QIODevice, Qt, Signal, Slot, QAbstractListModel -from python_qt_binding.QtGui import QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget, QCompleter -from python_qt_binding.QtSvg import QSvgGenerator - -import rosservice -import rostopic - -from .dotcode_pack import RosPackageGraphDotcodeGenerator -from qt_dotgraph.pydotfactory import PydotFactory -# from qt_dotgraph.pygraphvizfactory import PygraphvizFactory -from qt_dotgraph.dot_to_qt import DotToQtGenerator -from qt_gui_py_common.worker_thread import WorkerThread - -from rqt_gui_py.plugin import Plugin -from rqt_graph.interactive_graphics_view import InteractiveGraphicsView - - -class RepeatedWordCompleter(QCompleter): - """A completer that completes multiple times from a list""" - def pathFromIndex(self, index): - path = QCompleter.pathFromIndex(self, index) - lst = str(self.widget().text()).split(',') - if len(lst) > 1: - path = '%s, %s' % (','.join(lst[:-1]), path) - return path - - def splitPath(self, path): - path = str(path.split(',')[-1]).lstrip(' ') - return [path] - - -class StackageCompletionModel(QAbstractListModel): - """Ros package and stacknames""" - def __init__(self, linewidget, rospack, rosstack): - super(StackageCompletionModel, self).__init__(linewidget) - self.allnames = sorted(list(set(rospack.list() + rosstack.list()))) - self.allnames = self.allnames + ['-%s' % name for name in self.allnames] - - def rowCount(self, parent): - return len(self.allnames) - - def data(self, index, role): - # TODO: symbols to distinguish stacks from packages - if index.isValid() and (role == Qt.DisplayRole or role == Qt.EditRole): - return self.allnames[index.row()] - return None - - -class RosPackGraph(Plugin): - - _deferred_fit_in_view = Signal() - - def __init__(self, context): - super(RosPackGraph, self).__init__(context) - self.initialized = False - self._current_dotcode = None - self._update_thread = WorkerThread(self._update_thread_run, self._update_finished) - self._nodes = {} - self._edges = {} - self._options = {} - self._options_serialized = '' - - self.setObjectName('RosPackGraph') - - rospack = rospkg.RosPack() - rosstack = rospkg.RosStack() - - # factory builds generic dotcode items - self.dotcode_factory = PydotFactory() - # self.dotcode_factory = PygraphvizFactory() - # generator builds rosgraph - self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack) - # dot_to_qt transforms into Qt elements using dot layout - self.dot_to_qt = DotToQtGenerator() - - self._widget = QWidget() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui') - loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) - self._widget.setObjectName('RosPackGraphUi') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - self._scene = QGraphicsScene() - self._scene.setBackgroundBrush(Qt.white) - self._widget.graphics_view.setScene(self._scene) - - self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1) - self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2) - self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3) - self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4) - self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5) - self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) - - self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0) - self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1) - self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2) - self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) - - self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3) - self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2) - self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1) - self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) - - completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack) - completer = RepeatedWordCompleter(completionmodel, self) - completer.setCompletionMode(QCompleter.PopupCompletion) - completer.setWrapAround(True) - - completer.setCaseSensitivity(Qt.CaseInsensitive) - self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph) - self._widget.filter_line_edit.setCompleter(completer) - self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter) - - self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph) - self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph) - self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph) - self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph) - - self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) - self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph) - - self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph) - self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph) - self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) - self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) - - self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) - self._widget.load_dot_push_button.pressed.connect(self._load_dot) - self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_dot_push_button.pressed.connect(self._save_dot) - self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) - self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image')) - self._widget.save_as_image_push_button.pressed.connect(self._save_image) - - self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) - self._deferred_fit_in_view.emit() - - context.add_widget(self._widget) - - # If in either of following case, this turnes True - # - 1st filtering key is already input by user - # - filtering key is restored - self._filtering_started = False - - def shutdown_plugin(self): - self._update_thread.kill() - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('depth_combo_box_index', self._widget.depth_combo_box.currentIndex()) - instance_settings.set_value('directions_combo_box_index', self._widget.directions_combo_box.currentIndex()) - instance_settings.set_value('package_type_combo_box', self._widget.package_type_combo_box.currentIndex()) - instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text()) - instance_settings.set_value('with_stacks_state', self._widget.with_stacks_check_box.isChecked()) - instance_settings.set_value('hide_transitives_state', self._widget.hide_transitives_check_box.isChecked()) - instance_settings.set_value('mark_state', self._widget.mark_check_box.isChecked()) - instance_settings.set_value('colorize_state', self._widget.colorize_check_box.isChecked()) - instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) - instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) - - def restore_settings(self, plugin_settings, instance_settings): - _str_filter = instance_settings.value('filter_line_edit_text', '') - if (_str_filter == None or _str_filter == '') and \ - not self._filtering_started: - _str_filter = '(Separate pkgs by comma)' - else: - self._filtering_started = True - - self._widget.depth_combo_box.setCurrentIndex(int(instance_settings.value('depth_combo_box_index', 0))) - self._widget.directions_combo_box.setCurrentIndex(int(instance_settings.value('directions_combo_box_index', 0))) - self._widget.package_type_combo_box.setCurrentIndex(int(instance_settings.value('package_type_combo_box', 0))) - self._widget.filter_line_edit.setText(_str_filter) - self._widget.with_stacks_check_box.setChecked(instance_settings.value('with_stacks_state', True) in [True, 'true']) - self._widget.mark_check_box.setChecked(instance_settings.value('mark_state', True) in [True, 'true']) - self._widget.colorize_check_box.setChecked(instance_settings.value('colorize_state', False) in [True, 'true']) - self._widget.hide_transitives_check_box.setChecked(instance_settings.value('hide_transitives_state', False) in [True, 'true']) - self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) - self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) - self.initialized = True - self._refresh_rospackgraph() - - def _update_rospackgraph(self): - # re-enable controls customizing fetched ROS graph - self._widget.depth_combo_box.setEnabled(True) - self._widget.directions_combo_box.setEnabled(True) - self._widget.package_type_combo_box.setEnabled(True) - self._widget.filter_line_edit.setEnabled(True) - self._widget.with_stacks_check_box.setEnabled(True) - self._widget.mark_check_box.setEnabled(True) - self._widget.colorize_check_box.setEnabled(True) - self._widget.hide_transitives_check_box.setEnabled(True) - - self._refresh_rospackgraph(force_update=True) - - def _update_options(self): - self._options['depth'] = self._widget.depth_combo_box.itemData(self._widget.depth_combo_box.currentIndex()) - self._options['directions'] = self._widget.directions_combo_box.itemData(self._widget.directions_combo_box.currentIndex()) - self._options['package_types'] = self._widget.package_type_combo_box.itemData(self._widget.package_type_combo_box.currentIndex()) - self._options['with_stacks'] = self._widget.with_stacks_check_box.isChecked() - self._options['mark_selected'] = self._widget.mark_check_box.isChecked() - self._options['hide_transitives'] = self._widget.hide_transitives_check_box.isChecked() - # TODO: Allow different color themes - self._options['colortheme'] = True if self._widget.colorize_check_box.isChecked() else None - self._options['names'] = self._widget.filter_line_edit.text().split(',') - if self._options['names'] == [u'None']: - self._options['names'] = [] - self._options['highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked() else 1 - self._options['auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked() - - def _refresh_rospackgraph(self, force_update=False): - if not self.initialized: - return - - self._update_thread.kill() - - self._update_options() - - # avoid update if options did not change and force_update is not set - new_options_serialized = pickle.dumps(self._options) - if new_options_serialized == self._options_serialized and not force_update: - return - self._options_serialized = pickle.dumps(self._options) - - self._scene.setBackgroundBrush(Qt.lightGray) - - self._update_thread.start() - - # this runs in a non-gui thread, so don't access widgets here directly - def _update_thread_run(self): - self._update_graph(self._generate_dotcode()) - - @Slot() - def _update_finished(self): - self._scene.setBackgroundBrush(Qt.white) - self._redraw_graph_scene() - - # this runs in a non-gui thread, so don't access widgets here directly - def _generate_dotcode(self): - includes = [] - excludes = [] - for name in self._options['names']: - if name.strip().startswith('-'): - excludes.append(name.strip()[1:]) - else: - includes.append(name.strip()) - # orientation = 'LR' - descendants = True - ancestors = True - if self._options['directions'] == 1: - descendants = False - if self._options['directions'] == 0: - ancestors = False - return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory, - selected_names=includes, - excludes=excludes, - depth=self._options['depth'], - with_stacks=self._options['with_stacks'], - descendants=descendants, - ancestors=ancestors, - mark_selected=self._options['mark_selected'], - colortheme=self._options['colortheme'], - hide_transitives=self._options['hide_transitives'], - hide_wet=self._options['package_types'] == 1, - hide_dry=self._options['package_types'] == 2) - - # this runs in a non-gui thread, so don't access widgets here directly - def _update_graph(self, dotcode): - self._current_dotcode = dotcode - self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, self._options['highlight_level']) - - def _generate_tool_tip(self, url): - if url is not None and ':' in url: - item_type, item_path = url.split(':', 1) - if item_type == 'node': - tool_tip = 'Node:\n %s' % (item_path) - service_names = rosservice.get_service_list(node=item_path) - if service_names: - tool_tip += '\nServices:' - for service_name in service_names: - try: - service_type = rosservice.get_service_type(service_name) - tool_tip += '\n %s [%s]' % (service_name, service_type) - except rosservice.ROSServiceIOException, e: - tool_tip += '\n %s' % (e) - return tool_tip - elif item_type == 'topic': - topic_type, topic_name, _ = rostopic.get_topic_type(item_path) - return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) - return url - - def _redraw_graph_scene(self): - # remove items in order to not garbage nodes which will be continued to be used - for item in self._scene.items(): - self._scene.removeItem(item) - self._scene.clear() - for node_item in self._nodes.itervalues(): - self._scene.addItem(node_item) - for edge_items in self._edges.itervalues(): - for edge_item in edge_items: - edge_item.add_to_scene(self._scene) - - self._scene.setSceneRect(self._scene.itemsBoundingRect()) - if self._options['auto_fit']: - self._fit_in_view() - - def _load_dot(self, file_name=None): - if file_name is None: - file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - try: - fh = open(file_name, 'rb') - dotcode = fh.read() - fh.close() - except IOError: - return - - # disable controls customizing fetched ROS graph - self._widget.depth_combo_box.setEnabled(False) - self._widget.directions_combo_box.setEnabled(False) - self._widget.package_type_combo_box.setEnabled(False) - self._widget.filter_line_edit.setEnabled(False) - self._widget.with_stacks_check_box.setEnabled(False) - self._widget.mark_check_box.setEnabled(False) - self._widget.colorize_check_box.setEnabled(False) - self._widget.hide_transitives_check_box.setEnabled(False) - - self._update_graph(dotcode) - self._redraw_graph_scene() - - @Slot() - def _fit_in_view(self): - self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) - - def _save_dot(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rospackgraph.dot', self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - handle = QFile(file_name) - if not handle.open(QIODevice.WriteOnly | QIODevice.Text): - return - - handle.write(self._current_dotcode) - handle.close() - - def _save_svg(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rospackgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) - if file_name is None or file_name == '': - return - - generator = QSvgGenerator() - generator.setFileName(file_name) - generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) - - painter = QPainter(generator) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - - def _save_image(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rospackgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) - if file_name is None or file_name == '': - return - - img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) - painter = QPainter(img) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - img.save(file_name) - - def _clear_filter(self): - if not self._filtering_started: - self._widget.filter_line_edit.setText('') - self._filtering_started = True diff --git a/workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py b/workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py deleted file mode 100644 index 361a0154..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import unittest - -# get mock from pypi as 'mock' -from mock import Mock, patch - -from rqt_dep.dotcode_pack import RosPackageGraphDotcodeGenerator - - -PKG = 'rqt_deps' - - -class DotcodeGeneratorTest(unittest.TestCase): - - def test_packages_only(self): - with patch('rospkg.RosPack') as rospack: - with patch('rospkg.RosStack') as rosstack: - factoryMock = Mock() - graphMock = Mock() - rospack.list.return_value = [] - rosstack.list.return_value = [] - factoryMock.get_graph.return_value = graphMock - gen = RosPackageGraphDotcodeGenerator(rospack, rosstack) - graph = gen.generate_dotcode(factoryMock) - - rospack.list.assert_called() - rosstack.list.assert_called() - factoryMock.get_graph.assert_called_with(simplify=True, rank='same', ranksep=0.2, rankdir='TB') - factoryMock.create_dot.assert_called_with(graphMock) - - -if __name__ == '__main__': - import rosunit - rosunit.unitrun(PKG, 'test_packages_only', DotcodeGeneratorTest) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst deleted file mode 100644 index 35d79cf4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst +++ /dev/null @@ -1,114 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_graph -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* Remove repeated prefices from buttons -* Prefix all node and topic names with `n\_` and `t\_` respectively, to allow dot to distinguish them -* Contributors: Eric Wieser - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* fix duplicate rendering of statistics information (`#283 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- -* fix rendering of namespace boxes (`#266 `_) - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix compatibility with Groovy, use TopicStatistics only if available (`#252 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* add displaying of topic/connection statistics along edges (`#214 `_) -* using CATKIN_ENABLE_TESTING to optionally configure tests (`#220 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* modified zooming method to work better on high-res trackpads like Macbook Pros (`#187 `_) - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- -* Improve checkbox labels and tooltips wording. - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt deleted file mode 100644 index fefb8595..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_graph) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test/dotcode_test.py) -endif() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_graph - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox deleted file mode 100644 index a9f78acd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_graph provides a GUI plugin for visualizing the ROS computation graph. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_graph/package.xml b/workspace/src/rqt_common_plugins/rqt_graph/package.xml deleted file mode 100644 index b8cd2b1f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - rqt_graph - 0.3.13 - rqt_graph provides a GUI plugin for visualizing the ROS - computation graph.
- - Its components are made generic so that other packages - where you want to achieve graph representation can depend upon this pkg - (use rqt_dep to find out - the pkgs that depend. rqt_dep itself depends on rqt_graph too). - - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_graph - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dirk Thomas - - catkin - - python-rospkg - qt_dotgraph - rosgraph - rosgraph_msgs - roslib - rosnode - rospy - rosservice - rostopic - rqt_gui - rqt_gui_py - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_graph/plugin.xml b/workspace/src/rqt_common_plugins/rqt_graph/plugin.xml deleted file mode 100644 index 600eb9b1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin to visualize a ROS computation graph. - - - - - folder - Plugins related to introspection. - - - preferences-system-network - A Python GUI plugin for visualizing the ROS computation graph. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui b/workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui deleted file mode 100644 index f64b8305..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui +++ /dev/null @@ -1,301 +0,0 @@ - - - RosGraphWidget - - - true - - - - 0 - 0 - 1298 - 307 - - - - Node Graph - - - - - - - - Refresh ROS graph - - - - 16 - 16 - - - - - - - - - 0 - 0 - - - - Choose graph type - - - - - - - Namespace filter, comma separated list of names/regexp to include or exclude. Example: -/rqt.* - - - / - - - - - - - Topic filter, comma separated list of names/regexp to include or exclude. Example: -/tf - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Load DOT graph from file - - - - 16 - 16 - - - - - - - - Save as DOT graph - - - - 16 - 16 - - - - - - - - Save as SVG - - - - 16 - 16 - - - - - - - - Save as image - - - - 16 - 16 - - - - - - - - - - - - - - - - Group: - - - - - - - Show namespace boxes - - - Namespaces - - - - - - - Summarize action topics into one virtual topic node (named <action_server>/action_topics) - - - Actions - - - - - - - - - - Qt::Vertical - - - - - - - - - - Hide: - - - - - - - Hide topics without subscribers - - - Dead sinks - - - - - - - Hide topics with one connection only (implicates dead sinks) - - - Leaf topics - - - - - - - Hide common debugging nodes - - - Debug - - - - - - - - - - Qt::Vertical - - - - - - - Highlight incoming and outgoing connections on mouse over - - - Highlight - - - true - - - - - - - Automatically fit graph into view on update - - - Fit - - - true - - - - - - - Fit graph in view - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - QPainter::Antialiasing|QPainter::HighQualityAntialiasing|QPainter::SmoothPixmapTransform|QPainter::TextAntialiasing - - - QGraphicsView::AnchorViewCenter - - - - - - - - InteractiveGraphicsView - QGraphicsView -
rqt_graph.interactive_graphics_view
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph b/workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph deleted file mode 100755 index cfb71a9d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_graph.ros_graph.RosGraph')) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/setup.py b/workspace/src/rqt_common_plugins/rqt_graph/setup.py deleted file mode 100644 index de78f248..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_graph'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_graph'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/__init__.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py deleted file mode 100644 index af115a28..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py +++ /dev/null @@ -1,580 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# this is a modified version of rx/rxgraph/src/rxgraph/dotcode.py - -import re -import copy - -import rosgraph.impl.graph -import roslib -import math - -import rospy -import pydot - -# node/node connectivity -NODE_NODE_GRAPH = 'node_node' -# node/topic connections where an actual network connection exists -NODE_TOPIC_GRAPH = 'node_topic' -# all node/topic connections, even if no actual network connection -NODE_TOPIC_ALL_GRAPH = 'node_topic_all' - -QUIET_NAMES = ['/diag_agg', '/runtime_logger', '/pr2_dashboard', '/rviz', '/rosout', '/cpu_monitor', '/monitor', '/hd_monitor', '/rxloggerlevel', '/clock', '/rqt', '/statistics'] - -def _conv(n): - """Convert a node name to a valid dot name, which can't contain the leading space""" - if n.startswith(' '): - return 't_' + n[1:] - else: - return 'n_' + n - -def matches_any(name, patternlist): - if patternlist is None or len(patternlist) == 0: - return False - for pattern in patternlist: - if str(name).strip() == pattern: - return True - if re.match("^[a-zA-Z0-9_/]+$", pattern) is None: - if re.match(str(pattern), name.strip()) is not None: - return True - return False - - -class NodeConnections: - def __init__(self, incoming=None, outgoing=None): - self.incoming = incoming or [] - self.outgoing = outgoing or [] - - -class RosGraphDotcodeGenerator: - - # topic/topic -> graph.edge object - edges = dict([]) - - # ROS node name -> graph.node object - nodes = dict([]) - - def __init__(self): - try: - from rosgraph_msgs.msg import TopicStatistics - self.stats_sub = rospy.Subscriber('/statistics', TopicStatistics, self.statistics_callback) - except ImportError: - # not supported before Indigo - pass - - def statistics_callback(self,msg): - - # add connections (if new) - if not msg.node_sub in self.edges: - self.edges[msg.node_sub] = dict([]) - - if not msg.topic in self.edges[msg.node_sub]: - self.edges[msg.node_sub][msg.topic] = dict([]) - - self.edges[msg.node_sub][msg.topic][msg.node_pub] = msg - - def _get_max_traffic(self): - traffic = 10000 # start at 10kb - for sub in self.edges: - for topic in self.edges[sub]: - for pub in self.edges[sub][topic]: - traffic = max(traffic, self.edges[sub][topic][pub].traffic) - return traffic - - def _get_max_age(self): - age = 0.1 # start at 100ms - for sub in self.edges: - for topic in self.edges[sub]: - for pub in self.edges[sub][topic]: - age = max(age, self.edges[sub][topic][pub].stamp_age_mean.to_sec()) - return age - - def _get_max_age_on_topic(self, sub, topic): - age = 0.0 - for pub in self.edges[sub][topic]: - age = max(age, self.edges[sub][topic][pub].stamp_age_mean.to_sec()) - return age - - def _calc_edge_color(self, sub, topic, pub=None): - - age = 0.0 - - if pub is None: - age = self._get_max_age_on_topic(sub, topic) - elif sub in self.edges and topic in self.edges[sub] and pub in self.edges[sub][topic]: - age = self.edges[sub][topic][pub].stamp_age_mean.to_sec() - - if age == 0.0: - return [0, 0, 0] - - # calc coloring using the age - heat = max(age, 0) / self._get_max_age() - - # we assume that heat is normalized between 0.0 (green) and 1.0 (red) - # 0.0->green(0,255,0) to 0.5->yellow (255,255,0) to red 1.0(255,0,0) - if heat < 0: - red = 0 - green = 0 - elif heat <= 0.5: - red = int(heat * 255 * 2) - green = 255 - elif heat > 0.5: - red = 255 - green = 255 - int((heat - 0.5) * 255 * 2) - else: - red = 0 - green = 0 - return [red, green, 0] - - def _calc_edge_penwidth(self, sub, topic, pub=None): - if pub is None and sub in self.edges and topic in self.edges[sub]: - traffic = 0 - for p in self.edges[sub][topic]: - if pub is None or p == pub: - traffic += self.edges[sub][topic][p].traffic - - # calc penwidth using the traffic in kb/s - return int(traffic / self._get_max_traffic() * 5) - else: - return 1 - - def _calc_statistic_info(self, sub, topic, pub=None): - if pub is None and sub in self.edges and topic in self.edges[sub]: - conns = len(self.edges[sub][topic]) - if conns == 1: - pub = next(self.edges[sub][topic].iterkeys()) - else: - penwidth = self._calc_edge_penwidth(sub,topic) - color = self._calc_edge_color(sub,topic) - label = "("+str(conns) + " connections)" - return [label, penwidth, color] - - if sub in self.edges and topic in self.edges[sub] and pub in self.edges[sub][topic]: - penwidth = self._calc_edge_penwidth(sub,topic,pub) - color = self._calc_edge_color(sub,topic,pub) - period = self.edges[sub][topic][pub].period_mean.to_sec() - if period > 0.0: - freq = str(round(1.0 / period, 1)) - else: - freq = "?" - age = self.edges[sub][topic][pub].stamp_age_mean.to_sec() - age_string = "" - if age > 0.0: - age_string = " // " + str(round(age, 2) * 1000) + " ms" - label = freq + " Hz" + age_string - return [label, penwidth, color] - else: - return [None, None, None] - - def _add_edge(self, edge, dotcode_factory, dotgraph, is_topic=False): - if is_topic: - sub = edge.end - topic = edge.label - pub = edge.start - [stat_label, penwidth, color] = self._calc_statistic_info(sub, topic, pub) - if stat_label is not None: - temp_label = edge.label + "\\n" + stat_label - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=temp_label, url='topic:%s' % edge.label, penwidth=penwidth, color=color) - else: - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=edge.label, url='topic:%s' % edge.label) - else: - sub = edge.end.strip() - topic = edge.start.strip() - [stat_label, penwidth, color] = self._calc_statistic_info(sub, topic) - if stat_label is not None: - temp_label = edge.label + "\\n" + stat_label - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=temp_label, penwidth=penwidth, color=color) - else: - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=edge.label) - - - def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph, quiet): - if node in rosgraphinst.bad_nodes: - if quiet: - return '' - bn = rosgraphinst.bad_nodes[node] - if bn.type == rosgraph.impl.graph.BadNode.DEAD: - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=node, - shape="doublecircle", - url=node, - color="red") - else: - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=node, - shape="doublecircle", - url=node, - color="orange") - else: - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=node, - shape='ellipse', - url=node) - - def _add_topic_node(self, node, dotcode_factory, dotgraph, quiet): - label = rosgraph.impl.graph.node_topic(node) - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=label, - shape='box', - url="topic:%s" % label) - - def _quiet_filter(self, name): - # ignore viewers - for n in QUIET_NAMES: - if n in name: - return False - return True - - def quiet_filter_topic_edge(self, edge): - for quiet_label in ['/time', '/clock', '/rosout', '/statistics']: - if quiet_label == edge.label: - return False - return self._quiet_filter(edge.start) and self._quiet_filter(edge.end) - - def generate_namespaces(self, graph, graph_mode, quiet=False): - """ - Determine the namespaces of the nodes being displayed - """ - namespaces = [] - if graph_mode == NODE_NODE_GRAPH: - nodes = graph.nn_nodes - if quiet: - nodes = [n for n in nodes if not n in QUIET_NAMES] - namespaces = list(set([roslib.names.namespace(n) for n in nodes])) - - elif graph_mode == NODE_TOPIC_GRAPH or \ - graph_mode == NODE_TOPIC_ALL_GRAPH: - nn_nodes = graph.nn_nodes - nt_nodes = graph.nt_nodes - if quiet: - nn_nodes = [n for n in nn_nodes if not n in QUIET_NAMES] - nt_nodes = [n for n in nt_nodes if not n in QUIET_NAMES] - if nn_nodes or nt_nodes: - namespaces = [roslib.names.namespace(n) for n in nn_nodes] - # an annoyance with the rosgraph library is that it - # prepends a space to topic names as they have to have - # different graph node namees from nodes. we have to strip here - namespaces.extend([roslib.names.namespace(n[1:]) for n in nt_nodes]) - - return list(set(namespaces)) - - def _filter_orphaned_edges(self, edges, nodes): - nodenames = [str(n).strip() for n in nodes] - # currently using and rule as the or rule generates orphan nodes with the current logic - return [e for e in edges if e.start.strip() in nodenames and e.end.strip() in nodenames] - - def _filter_orphaned_topics(self, nt_nodes, edges): - '''remove topic graphnodes without connected ROS nodes''' - removal_nodes = [] - for n in nt_nodes: - keep = False - for e in edges: - if (e.start.strip() == str(n).strip() or e.end.strip() == str(n).strip()): - keep = True - break - if not keep: - removal_nodes.append(n) - for n in removal_nodes: - nt_nodes.remove(n) - return nt_nodes - - def _split_filter_string(self, ns_filter): - '''splits a string after each comma, and treats tokens with leading dash as exclusions. - Adds .* as inclusion if no other inclusion option was given''' - includes = [] - excludes = [] - for name in ns_filter.split(','): - if name.strip().startswith('-'): - excludes.append(name.strip()[1:]) - else: - includes.append(name.strip()) - if includes == [] or includes == ['/'] or includes == ['']: - includes = ['.*'] - return includes, excludes - - def _get_node_edge_map(self, edges): - '''returns a dict mapping node name to edge objects partitioned in incoming and outgoing edges''' - node_connections = {} - for edge in edges: - if not edge.start in node_connections: - node_connections[edge.start] = NodeConnections() - if not edge.end in node_connections: - node_connections[edge.end] = NodeConnections() - node_connections[edge.start].outgoing.append(edge) - node_connections[edge.end].incoming.append(edge) - return node_connections - - def _filter_leaf_topics(self, - nodes_in, - edges_in, - node_connections, - hide_single_connection_topics, - hide_dead_end_topics): - ''' - removes certain ending topic nodes and their edges from list of nodes and edges - - @param hide_single_connection_topics: if true removes topics that are only published/subscribed by one node - @param hide_dead_end_topics: if true removes topics having only publishers - ''' - if not hide_dead_end_topics and not hide_single_connection_topics: - return nodes_in, edges_in - # do not manipulate incoming structures - nodes = copy.copy(nodes_in) - edges = copy.copy(edges_in) - removal_nodes = [] - for n in nodes: - if n in node_connections: - node_edges = [] - has_out_edges = False - node_edges.extend(node_connections[n].outgoing) - if len(node_connections[n].outgoing) > 0: - has_out_edges = True - node_edges.extend(node_connections[n].incoming) - if ((hide_single_connection_topics and len(node_edges) < 2) or - (hide_dead_end_topics and not has_out_edges)): - removal_nodes.append(n) - for e in node_edges: - if e in edges: - edges.remove(e) - for n in removal_nodes: - nodes.remove(n) - return nodes, edges - - def _accumulate_action_topics(self, nodes_in, edges_in, node_connections): - '''takes topic nodes, edges and node connections. - Returns topic nodes where action topics have been removed, - edges where the edges to action topics have been removed, and - a map with the connection to each virtual action topic node''' - removal_nodes = [] - action_nodes = {} - # do not manipulate incoming structures - nodes = copy.copy(nodes_in) - edges = copy.copy(edges_in) - for n in nodes: - if str(n).endswith('/feedback'): - prefix = str(n)[:-len('/feedback')].strip() - action_topic_nodes = [] - action_topic_edges_out = set() - action_topic_edges_in = set() - for suffix in ['/status', '/result', '/goal', '/cancel', '/feedback']: - for n2 in nodes: - if str(n2).strip() == prefix + suffix: - action_topic_nodes.append(n2) - if n2 in node_connections: - action_topic_edges_out.update(node_connections[n2].outgoing) - action_topic_edges_in.update(node_connections[n2].incoming) - if len(action_topic_nodes) == 5: - # found action - removal_nodes.extend(action_topic_nodes) - for e in action_topic_edges_out: - if e in edges: - edges.remove(e) - for e in action_topic_edges_in: - if e in edges: - edges.remove(e) - action_nodes[prefix] = {'topics': action_topic_nodes, - 'outgoing': action_topic_edges_out, - 'incoming': action_topic_edges_in} - for n in removal_nodes: - nodes.remove(n) - return nodes, edges, action_nodes - - def generate_dotgraph(self, - rosgraphinst, - ns_filter, - topic_filter, - graph_mode, - dotcode_factory, - hide_single_connection_topics=False, - hide_dead_end_topics=False, - cluster_namespaces_level=0, - accumulate_actions=True, - orientation='LR', - rank='same', # None, same, min, max, source, sink - ranksep=0.2, # vertical distance between layers - rankdir='TB', # direction of layout (TB top > bottom, LR left > right) - simplify=True, # remove double edges - quiet=False): - """ - See generate_dotcode - """ - includes, excludes = self._split_filter_string(ns_filter) - topic_includes, topic_excludes = self._split_filter_string(topic_filter) - - nn_nodes = [] - nt_nodes = [] - # create the node definitions - if graph_mode == NODE_NODE_GRAPH: - nn_nodes = rosgraphinst.nn_nodes - nn_nodes = [n for n in nn_nodes if matches_any(n, includes) and not matches_any(n, excludes)] - edges = rosgraphinst.nn_edges - edges = [e for e in edges if matches_any(e.label, topic_includes) and not matches_any(e.label, topic_excludes)] - - elif graph_mode == NODE_TOPIC_GRAPH or \ - graph_mode == NODE_TOPIC_ALL_GRAPH: - nn_nodes = rosgraphinst.nn_nodes - nt_nodes = rosgraphinst.nt_nodes - nn_nodes = [n for n in nn_nodes if matches_any(n, includes) and not matches_any(n, excludes)] - nt_nodes = [n for n in nt_nodes if matches_any(n, topic_includes) and not matches_any(n, topic_excludes)] - - # create the edge definitions, unwrap EdgeList objects into python lists - if graph_mode == NODE_TOPIC_GRAPH: - edges = [e for e in rosgraphinst.nt_edges] - else: - edges = [e for e in rosgraphinst.nt_all_edges] - - if quiet: - nn_nodes = filter(self._quiet_filter, nn_nodes) - nt_nodes = filter(self._quiet_filter, nt_nodes) - if graph_mode == NODE_NODE_GRAPH: - edges = filter(self.quiet_filter_topic_edge, edges) - - # for accumulating actions topics - action_nodes = {} - - if graph_mode != NODE_NODE_GRAPH and (hide_single_connection_topics or hide_dead_end_topics or accumulate_actions): - # maps outgoing and incoming edges to nodes - node_connections = self._get_node_edge_map(edges) - - nt_nodes, edges = self._filter_leaf_topics(nt_nodes, - edges, - node_connections, - hide_single_connection_topics, - hide_dead_end_topics) - - if accumulate_actions: - nt_nodes, edges, action_nodes = self._accumulate_action_topics(nt_nodes, edges, node_connections) - - edges = self._filter_orphaned_edges(edges, list(nn_nodes) + list(nt_nodes)) - nt_nodes = self._filter_orphaned_topics(nt_nodes, edges) - - # create the graph - # result = "digraph G {\n rankdir=%(orientation)s;\n%(nodes_str)s\n%(edges_str)s}\n" % vars() - dotgraph = dotcode_factory.get_graph(rank=rank, - ranksep=ranksep, - simplify=simplify, - rankdir=orientation) - - ACTION_TOPICS_SUFFIX = '/action_topics' - namespace_clusters = {} - for n in (nt_nodes or []) + [action_prefix + ACTION_TOPICS_SUFFIX for (action_prefix, _) in action_nodes.items()]: - # cluster topics with same namespace - if (cluster_namespaces_level > 0 and - str(n).count('/') > 1 and - len(str(n).split('/')[1]) > 0): - namespace = str(n).split('/')[1] - if namespace not in namespace_clusters: - namespace_clusters[namespace] = dotcode_factory.add_subgraph_to_graph(dotgraph, namespace, rank=rank, rankdir=orientation, simplify=simplify) - self._add_topic_node(n, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet) - else: - self._add_topic_node(n, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) - - # for ROS node, if we have created a namespace clusters for - # one of its peer topics, drop it into that cluster - if nn_nodes is not None: - for n in nn_nodes: - if (cluster_namespaces_level > 0 and - str(n).count('/') >= 1 and - len(str(n).split('/')[1]) > 0): - namespace = str(n).split('/')[1] - if namespace not in namespace_clusters: - namespace_clusters[namespace] = dotcode_factory.add_subgraph_to_graph(dotgraph, namespace, rank=rank, rankdir=orientation, simplify=simplify) - self._add_node(n, rosgraphinst=rosgraphinst, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet) - else: - self._add_node(n, rosgraphinst=rosgraphinst, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) - - for e in edges: - self._add_edge(e, dotcode_factory, dotgraph=dotgraph, is_topic=(graph_mode == NODE_NODE_GRAPH)) - - for (action_prefix, node_connections) in action_nodes.items(): - for out_edge in node_connections.get('outgoing', []): - dotcode_factory.add_edge_to_graph(dotgraph, _conv(action_prefix + ACTION_TOPICS_SUFFIX), _conv(out_edge.end)) - for in_edge in node_connections.get('incoming', []): - dotcode_factory.add_edge_to_graph(dotgraph, _conv(in_edge.start), _conv(action_prefix + ACTION_TOPICS_SUFFIX)) - return dotgraph - - def generate_dotcode(self, - rosgraphinst, - ns_filter, - topic_filter, - graph_mode, - dotcode_factory, - hide_single_connection_topics=False, - hide_dead_end_topics=False, - cluster_namespaces_level=0, - accumulate_actions=True, - orientation='LR', - rank='same', # None, same, min, max, source, sink - ranksep=0.2, # vertical distance between layers - rankdir='TB', # direction of layout (TB top > bottom, LR left > right) - simplify=True, # remove double edges - quiet=False): - """ - @param rosgraphinst: RosGraph instance - @param ns_filter: nodename filter - @type ns_filter: string - @param topic_filter: topicname filter - @type ns_filter: string - @param graph_mode str: NODE_NODE_GRAPH | NODE_TOPIC_GRAPH | NODE_TOPIC_ALL_GRAPH - @type graph_mode: str - @param orientation: rankdir value (see ORIENTATIONS dict) - @type dotcode_factory: object - @param dotcode_factory: abstract factory manipulating dot language objects - @param hide_single_connection_topics: if true remove topics with just one connection - @param hide_dead_end_topics: if true remove topics with publishers only - @param cluster_namespaces_level: if > 0 places box around members of same namespace (TODO: multiple namespace layers) - @param accumulate_actions: if true each 5 action topic graph nodes are shown as single graph node - @return: dotcode generated from graph singleton - @rtype: str - """ - dotgraph = self.generate_dotgraph(rosgraphinst=rosgraphinst, - ns_filter=ns_filter, - topic_filter=topic_filter, - graph_mode=graph_mode, - dotcode_factory=dotcode_factory, - hide_single_connection_topics=hide_single_connection_topics, - hide_dead_end_topics=hide_dead_end_topics, - cluster_namespaces_level=cluster_namespaces_level, - accumulate_actions=accumulate_actions, - orientation=orientation, - rank=rank, - ranksep=ranksep, - rankdir=rankdir, - simplify=simplify, - quiet=quiet) - dotcode = dotcode_factory.create_dot(dotgraph) - return dotcode diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py deleted file mode 100644 index 7d038e45..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright (c) 2011, Dirk Thomas, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division - -from python_qt_binding.QtCore import QPointF, QRectF, Qt -from python_qt_binding.QtGui import QGraphicsView, QTransform - - -class InteractiveGraphicsView(QGraphicsView): - - def __init__(self, parent=None): - super(InteractiveGraphicsView, self).__init__(parent) - self.setObjectName('InteractiveGraphicsView') - - self._last_pan_point = None - self._last_scene_center = None - - def mousePressEvent(self, mouse_event): - self._last_pan_point = mouse_event.pos() - self._last_scene_center = self._map_to_scene_f(QRectF(self.frameRect()).center()) - self.setCursor(Qt.ClosedHandCursor) - - def mouseReleaseEvent(self, mouse_event): - self.setCursor(Qt.OpenHandCursor) - self._last_pan_point = None - - def mouseMoveEvent(self, mouse_event): - if self._last_pan_point is not None: - delta_scene = self.mapToScene(mouse_event.pos()) - self.mapToScene(self._last_pan_point) - if not delta_scene.isNull(): - self.centerOn(self._last_scene_center - delta_scene) - self._last_scene_center -= delta_scene - self._last_pan_point = mouse_event.pos() - QGraphicsView.mouseMoveEvent(self, mouse_event) - - def wheelEvent(self, wheel_event): - if wheel_event.modifiers() == Qt.NoModifier: - delta = wheel_event.delta() - delta = max(min(delta, 480), -480) - mouse_before_scale_in_scene = self.mapToScene(wheel_event.pos()) - - scale_factor = 1 + (0.2 * (delta / 120.0)) - scaling = QTransform(scale_factor, 0, 0, scale_factor, 0, 0) - self.setTransform(self.transform() * scaling) - - mouse_after_scale_in_scene = self.mapToScene(wheel_event.pos()) - center_in_scene = self.mapToScene(self.frameRect().center()) - self.centerOn(center_in_scene + mouse_before_scale_in_scene - mouse_after_scale_in_scene) - - wheel_event.accept() - else: - QGraphicsView.wheelEvent(self, wheel_event) - - def _map_to_scene_f(self, pointf): - point = pointf.toPoint() - if pointf.x() == point.x() and pointf.y() == point.y(): - # map integer coordinates - return self.mapToScene(point) - elif pointf.x() == point.x(): - # map integer x and decimal y coordinates - pointA = self.mapToScene((pointf + QPointF(0, -0.5)).toPoint()) - pointB = self.mapToScene((pointf + QPointF(0, 0.5)).toPoint()) - return (pointA + pointB) / 2.0 - elif pointf.y() == point.y(): - # map decimal x and integer y and coordinates - pointA = self.mapToScene((pointf + QPointF(-0.5, 0)).toPoint()) - pointB = self.mapToScene((pointf + QPointF(0.5, 0)).toPoint()) - return (pointA + pointB) / 2.0 - else: - # map decimal coordinates - pointA = self.mapToScene((pointf + QPointF(-0.5, -0.5)).toPoint()) - pointB = self.mapToScene((pointf + QPointF(-0.5, 0.5)).toPoint()) - pointC = self.mapToScene((pointf + QPointF(0.5, -0.5)).toPoint()) - pointD = self.mapToScene((pointf + QPointF(0.5, 0.5)).toPoint()) - return (pointA + pointB + pointC + pointD) / 4.0 diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py deleted file mode 100644 index bb2708f2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py +++ /dev/null @@ -1,368 +0,0 @@ -# Copyright (c) 2011, Dirk Thomas, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QAbstractListModel, QFile, QIODevice, Qt, Signal -from python_qt_binding.QtGui import QCompleter, QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget -from python_qt_binding.QtSvg import QSvgGenerator - -import rosgraph.impl.graph -import rosservice -import rostopic - -from qt_dotgraph.dot_to_qt import DotToQtGenerator -# pydot requires some hacks -from qt_dotgraph.pydotfactory import PydotFactory -from rqt_gui_py.plugin import Plugin -# TODO: use pygraphviz instead, but non-deterministic layout will first be resolved in graphviz 2.30 -# from qtgui_plugin.pygraphvizfactory import PygraphvizFactory - -from .dotcode import RosGraphDotcodeGenerator, NODE_NODE_GRAPH, NODE_TOPIC_ALL_GRAPH, NODE_TOPIC_GRAPH -from .interactive_graphics_view import InteractiveGraphicsView - - -class RepeatedWordCompleter(QCompleter): - """A completer that completes multiple times from a list""" - def init(self, parent=None): - QCompleter.init(self, parent) - - def pathFromIndex(self, index): - path = QCompleter.pathFromIndex(self, index) - lst = str(self.widget().text()).split(',') - if len(lst) > 1: - path = '%s, %s' % (','.join(lst[:-1]), path) - return path - - def splitPath(self, path): - path = str(path.split(',')[-1]).lstrip(' ') - return [path] - - -class NamespaceCompletionModel(QAbstractListModel): - """Ros package and stacknames""" - def __init__(self, linewidget, topics_only): - super(NamespaceCompletionModel, self).__init__(linewidget) - self.names = [] - - def refresh(self, names): - namesset = set() - for n in names: - namesset.add(str(n).strip()) - namesset.add("-%s" % (str(n).strip())) - self.names = sorted(namesset) - - def rowCount(self, parent): - return len(self.names) - - def data(self, index, role): - if index.isValid() and (role == Qt.DisplayRole or role == Qt.EditRole): - return self.names[index.row()] - return None - - -class RosGraph(Plugin): - - _deferred_fit_in_view = Signal() - - def __init__(self, context): - super(RosGraph, self).__init__(context) - self.initialized = False - self.setObjectName('RosGraph') - - self._graph = None - self._current_dotcode = None - - self._widget = QWidget() - - # factory builds generic dotcode items - self.dotcode_factory = PydotFactory() - # self.dotcode_factory = PygraphvizFactory() - # generator builds rosgraph - self.dotcode_generator = RosGraphDotcodeGenerator() - # dot_to_qt transforms into Qt elements using dot layout - self.dot_to_qt = DotToQtGenerator() - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui') - loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) - self._widget.setObjectName('RosGraphUi') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - self._scene = QGraphicsScene() - self._scene.setBackgroundBrush(Qt.white) - self._widget.graphics_view.setScene(self._scene) - - self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH) - self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH) - self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH) - self._widget.graph_type_combo_box.setCurrentIndex(0) - self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph) - - self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False) - completer = RepeatedWordCompleter(self.node_completionmodel, self) - completer.setCompletionMode(QCompleter.PopupCompletion) - completer.setWrapAround(True) - completer.setCaseSensitivity(Qt.CaseInsensitive) - self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph) - self._widget.filter_line_edit.setCompleter(completer) - - self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False) - topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self) - topic_completer.setCompletionMode(QCompleter.PopupCompletion) - topic_completer.setWrapAround(True) - topic_completer.setCaseSensitivity(Qt.CaseInsensitive) - self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph) - self._widget.topic_filter_line_edit.setCompleter(topic_completer) - - self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph) - - self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) - self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph) - - self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view) - self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view) - self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) - self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) - - self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) - self._widget.load_dot_push_button.pressed.connect(self._load_dot) - self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_dot_push_button.pressed.connect(self._save_dot) - self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) - self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image')) - self._widget.save_as_image_push_button.pressed.connect(self._save_image) - - self._update_rosgraph() - self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) - self._deferred_fit_in_view.emit() - - context.add_widget(self._widget) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex()) - instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text()) - instance_settings.set_value('topic_filter_line_edit_text', self._widget.topic_filter_line_edit.text()) - instance_settings.set_value('namespace_cluster_check_box_state', self._widget.namespace_cluster_check_box.isChecked()) - instance_settings.set_value('actionlib_check_box_state', self._widget.actionlib_check_box.isChecked()) - instance_settings.set_value('dead_sinks_check_box_state', self._widget.dead_sinks_check_box.isChecked()) - instance_settings.set_value('leaf_topics_check_box_state', self._widget.leaf_topics_check_box.isChecked()) - instance_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked()) - instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) - instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.graph_type_combo_box.setCurrentIndex(int(instance_settings.value('graph_type_combo_box_index', 0))) - self._widget.filter_line_edit.setText(instance_settings.value('filter_line_edit_text', '/')) - self._widget.topic_filter_line_edit.setText(instance_settings.value('topic_filter_line_edit_text', '/')) - self._widget.namespace_cluster_check_box.setChecked(instance_settings.value('namespace_cluster_check_box_state', True) in [True, 'true']) - self._widget.actionlib_check_box.setChecked(instance_settings.value('actionlib_check_box_state', True) in [True, 'true']) - self._widget.dead_sinks_check_box.setChecked(instance_settings.value('dead_sinks_check_box_state', True) in [True, 'true']) - self._widget.leaf_topics_check_box.setChecked(instance_settings.value('leaf_topics_check_box_state', True) in [True, 'true']) - self._widget.quiet_check_box.setChecked(instance_settings.value('quiet_check_box_state', True) in [True, 'true']) - self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) - self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) - self.initialized = True - self._refresh_rosgraph() - - def _update_rosgraph(self): - # re-enable controls customizing fetched ROS graph - self._widget.graph_type_combo_box.setEnabled(True) - self._widget.filter_line_edit.setEnabled(True) - self._widget.topic_filter_line_edit.setEnabled(True) - self._widget.namespace_cluster_check_box.setEnabled(True) - self._widget.actionlib_check_box.setEnabled(True) - self._widget.dead_sinks_check_box.setEnabled(True) - self._widget.leaf_topics_check_box.setEnabled(True) - self._widget.quiet_check_box.setEnabled(True) - - self._graph = rosgraph.impl.graph.Graph() - self._graph.set_master_stale(5.0) - self._graph.set_node_stale(5.0) - self._graph.update() - self.node_completionmodel.refresh(self._graph.nn_nodes) - self.topic_completionmodel.refresh(self._graph.nt_nodes) - self._refresh_rosgraph() - - def _refresh_rosgraph(self): - if not self.initialized: - return - self._update_graph_view(self._generate_dotcode()) - - def _generate_dotcode(self): - ns_filter = self._widget.filter_line_edit.text() - topic_filter = self._widget.topic_filter_line_edit.text() - graph_mode = self._widget.graph_type_combo_box.itemData(self._widget.graph_type_combo_box.currentIndex()) - orientation = 'LR' - if self._widget.namespace_cluster_check_box.isChecked(): - namespace_cluster = 1 - else: - namespace_cluster = 0 - accumulate_actions = self._widget.actionlib_check_box.isChecked() - hide_dead_end_topics = self._widget.dead_sinks_check_box.isChecked() - hide_single_connection_topics = self._widget.leaf_topics_check_box.isChecked() - quiet = self._widget.quiet_check_box.isChecked() - - return self.dotcode_generator.generate_dotcode( - rosgraphinst=self._graph, - ns_filter=ns_filter, - topic_filter=topic_filter, - graph_mode=graph_mode, - hide_single_connection_topics=hide_single_connection_topics, - hide_dead_end_topics=hide_dead_end_topics, - cluster_namespaces_level=namespace_cluster, - accumulate_actions=accumulate_actions, - dotcode_factory=self.dotcode_factory, - orientation=orientation, - quiet=quiet) - - def _update_graph_view(self, dotcode): - if dotcode == self._current_dotcode: - return - self._current_dotcode = dotcode - self._redraw_graph_view() - - def _generate_tool_tip(self, url): - if url is not None and ':' in url: - item_type, item_path = url.split(':', 1) - if item_type == 'node': - tool_tip = 'Node:\n %s' % (item_path) - service_names = rosservice.get_service_list(node=item_path) - if service_names: - tool_tip += '\nServices:' - for service_name in service_names: - try: - service_type = rosservice.get_service_type(service_name) - tool_tip += '\n %s [%s]' % (service_name, service_type) - except rosservice.ROSServiceIOException as e: - tool_tip += '\n %s' % (e) - return tool_tip - elif item_type == 'topic': - topic_type, topic_name, _ = rostopic.get_topic_type(item_path) - return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) - return url - - def _redraw_graph_view(self): - self._scene.clear() - - if self._widget.highlight_connections_check_box.isChecked(): - highlight_level = 3 - else: - highlight_level = 1 - - # layout graph and create qt items - (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, - highlight_level=highlight_level, - same_label_siblings=True) - - for node_item in nodes.itervalues(): - self._scene.addItem(node_item) - for edge_items in edges.itervalues(): - for edge_item in edge_items: - edge_item.add_to_scene(self._scene) - - self._scene.setSceneRect(self._scene.itemsBoundingRect()) - if self._widget.auto_fit_graph_check_box.isChecked(): - self._fit_in_view() - - def _load_dot(self, file_name=None): - if file_name is None: - file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - try: - fh = open(file_name, 'rb') - dotcode = fh.read() - fh.close() - except IOError: - return - - # disable controls customizing fetched ROS graph - self._widget.graph_type_combo_box.setEnabled(False) - self._widget.filter_line_edit.setEnabled(False) - self._widget.topic_filter_line_edit.setEnabled(False) - self._widget.namespace_cluster_check_box.setEnabled(False) - self._widget.actionlib_check_box.setEnabled(False) - self._widget.dead_sinks_check_box.setEnabled(False) - self._widget.leaf_topics_check_box.setEnabled(False) - self._widget.quiet_check_box.setEnabled(False) - - self._update_graph_view(dotcode) - - def _fit_in_view(self): - self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) - - def _save_dot(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rosgraph.dot', self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - handle = QFile(file_name) - if not handle.open(QIODevice.WriteOnly | QIODevice.Text): - return - - handle.write(self._current_dotcode) - handle.close() - - def _save_svg(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) - if file_name is None or file_name == '': - return - - generator = QSvgGenerator() - generator.setFileName(file_name) - generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) - - painter = QPainter(generator) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - - def _save_image(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) - if file_name is None or file_name == '': - return - - img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) - painter = QPainter(img) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - img.save(file_name) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py b/workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py deleted file mode 100644 index dd2c5b71..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import unittest -import rospkg - -import os -from rqt_graph.dotcode import RosGraphDotcodeGenerator - -PKG='rqt_graph' - -class DotcodeTest(unittest.TestCase): - - def test_split_filter_empty(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('') - self.assertEqual(['.*'], inc) - self.assertEqual(0, len(exc)) - inc, exc = gen._split_filter_string('/') - self.assertEqual(['.*'], inc) - self.assertEqual(0, len(exc)) - - - def test_split_filter_includes(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('foo') - self.assertEqual(['foo'], inc) - self.assertEqual(0, len(exc)) - inc, exc = gen._split_filter_string('foo,bar') - self.assertEqual(['foo', 'bar'], inc) - self.assertEqual(0, len(exc)) - - def test_split_filter_excludes(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('-foo') - self.assertEqual(['.*'], inc) - self.assertEqual(['foo'], exc) - inc, exc = gen._split_filter_string('-foo,-bar') - self.assertEqual(['.*'], inc) - self.assertEqual(['foo', 'bar'], exc) - - def test_split_filter_both(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('-foo , bar ,baz, -bam') - self.assertEqual(['bar', 'baz'], inc) - self.assertEqual(['foo', 'bam'], exc) - - class MockEdge(): - def __init__(self, start, end): - self.start = start - self.end = end - - def test_get_node_edge_map(self): - gen = RosGraphDotcodeGenerator() - e1 = self.MockEdge('foo', 'bar') - nmap = gen._get_node_edge_map([e1]) - self.assertEqual(2, len(nmap)) - self.assertTrue('foo' in nmap) - self.assertTrue('bar' in nmap) - self.assertEqual([], nmap['foo'].incoming) - self.assertEqual([e1], nmap['foo'].outgoing) - self.assertEqual([e1], nmap['bar'].incoming) - self.assertEqual([], nmap['bar'].outgoing) - - def test_get_node_edge_map(self): - gen = RosGraphDotcodeGenerator() - e1 = self.MockEdge('foo', 'bar') - e2 = self.MockEdge('bar', 'foo') - e3 = self.MockEdge('foo', 'pam') - nmap = gen._get_node_edge_map([e1, e2, e3]) - self.assertEqual(3, len(nmap)) - self.assertTrue('foo' in nmap) - self.assertTrue('bar' in nmap) - self.assertTrue('pam' in nmap) - self.assertEqual([e2], nmap['foo'].incoming) - self.assertEqual([e1, e3], nmap['foo'].outgoing) - self.assertEqual([e1], nmap['bar'].incoming) - self.assertEqual([e2], nmap['bar'].outgoing) - - def test_filter_leaf_topics_single_connection(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo', 'bar', 'pam', 'boo'] - e1 = self.MockEdge('n1', 'foo') - e2 = self.MockEdge('n2', 'foo') - e3 = self.MockEdge('n3', 'bar') - e4 = self.MockEdge('bar', 'n4') - e5 = self.MockEdge('n5', 'pam') - e6 = self.MockEdge('boo', 'n6') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - print(node_connections) - rnodes, redges = gen._filter_leaf_topics(topic_nodes, - edges, - node_connections, - True, #hide_single_connection_topics, - False #hide_dead_end_topics - ) - self.assertEqual(['foo', 'bar'], rnodes) - self.assertEqual([e1, e2, e3, e4], redges) - self.assertEqual(['foo', 'bar', 'pam', 'boo'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - def test_filter_leaf_topics_dead_end(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo', 'bar', 'pam', 'boo'] - e1 = self.MockEdge('n1', 'foo') - e2 = self.MockEdge('n2', 'foo') - e3 = self.MockEdge('n3', 'bar') - e4 = self.MockEdge('bar', 'n4') - e5 = self.MockEdge('n5', 'pam') - e6 = self.MockEdge('boo', 'n6') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - print(node_connections) - rnodes, redges = gen._filter_leaf_topics(topic_nodes, - edges, - node_connections, - False, #hide_single_connection_topics, - True #hide_dead_end_topics - ) - self.assertEqual(['bar', 'boo'], rnodes) - self.assertEqual([e3, e4, e6], redges) - self.assertEqual(['foo', 'bar', 'pam', 'boo'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - - def test_filter_leaf_topics_both(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo', 'bar', 'pam', 'boo'] - e1 = self.MockEdge('n1', 'foo') - e2 = self.MockEdge('n2', 'foo') - e3 = self.MockEdge('n3', 'bar') - e4 = self.MockEdge('bar', 'n4') - e5 = self.MockEdge('n5', 'pam') - e6 = self.MockEdge('boo', 'n6') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - print(node_connections) - rnodes, redges = gen._filter_leaf_topics(topic_nodes, - edges, - node_connections, - True, #hide_single_connection_topics, - True #hide_dead_end_topics - ) - self.assertEqual(['bar'], rnodes) - self.assertEqual([e3, e4], redges) - self.assertEqual(['foo', 'bar', 'pam', 'boo'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - def test_accumulate_action_topics(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'] - e1 = self.MockEdge('n1', 'foo/feedback') - e2 = self.MockEdge('n1', 'foo/goal') - e3 = self.MockEdge('n1', 'foo/cancel') - e4 = self.MockEdge('n1', 'foo/result') - e5 = self.MockEdge('n1', 'foo/status') - e6 = self.MockEdge('n2', 'bar') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - rnodes, redges, raction_nodes = gen._accumulate_action_topics(topic_nodes, edges, node_connections) - self.assertEqual(1, len(rnodes)) # node 'bar' - self.assertEqual(1, len(redges)) - self.assertEqual(1, len(raction_nodes)) - self.assertEqual(['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - -if __name__ == '__main__': - import rosunit - rosunit.unitrun(PKG, 'dotcode_test', DotcodeTest) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst deleted file mode 100644 index a82b4842..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst +++ /dev/null @@ -1,115 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_image_view -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* use proper icon for images -* add optional topic argument to rqt_image_view -* fix width of save-as-image button -* Contributors: Dirk Thomas, Vincent Rabaud - -0.3.12 (2015-07-24) -------------------- -* Added button to save current image to file -* Contributors: Dirk Thomas - -0.3.11 (2015-04-30) -------------------- -* fix image shrinking problem (`#291 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* list image transport topics if parent image topic is not available (`#215 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* properly handle aligned images -* wrap cv calls in try-catch-block (`#201 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* fix event handling for rqt_image_view enabling to run multiple instances simultaneously (`#66 `_) -* add rqt_image_view to global bin (`#168 `_) - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- -* Optimized by taking more advantage of cv_bridge - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt deleted file mode 100644 index addc49af..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(rqt_image_view) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED COMPONENTS rqt_gui rqt_gui_cpp image_transport sensor_msgs cv_bridge) - -find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) - -include(${QT_USE_FILE}) - -set(rqt_image_view_SRCS - src/rqt_image_view/image_view.cpp - src/rqt_image_view/ratio_layouted_frame.cpp -) - -set(rqt_image_view_HDRS - include/rqt_image_view/image_view.h - include/rqt_image_view/ratio_layouted_frame.h -) - -set(rqt_image_view_UIS - src/rqt_image_view/image_view.ui -) - -set(rqt_image_view_INCLUDE_DIRECTORIES - include - ${CMAKE_CURRENT_BINARY_DIR} -) - -catkin_package( - INCLUDE_DIRS ${rqt_image_view_INCLUDE_DIRECTORIES} - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS rqt_gui rqt_gui_cpp image_transport sensor_msgs cv_bridge -) -catkin_python_setup() - -qt4_wrap_cpp(rqt_image_view_MOCS ${rqt_image_view_HDRS}) -qt4_wrap_ui(rqt_image_view_UIS_H ${rqt_image_view_UIS}) - -include_directories(${rqt_image_view_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS}) -add_library(${PROJECT_NAME} ${rqt_image_view_SRCS} ${rqt_image_view_MOCS} ${rqt_image_view_UIS_H}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - -find_package(class_loader) -class_loader_hide_library_symbols(${PROJECT_NAME}) - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install(PROGRAMS scripts/rqt_image_view - DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) -install(PROGRAMS scripts/rqt_image_view - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h b/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h deleted file mode 100644 index 005a9500..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef rqt_image_view__ImageView_H -#define rqt_image_view__ImageView_H - -#include - -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include - -namespace rqt_image_view { - -class ImageView - : public rqt_gui_cpp::Plugin -{ - - Q_OBJECT - -public: - - ImageView(); - - virtual void initPlugin(qt_gui_cpp::PluginContext& context); - - virtual void shutdownPlugin(); - - virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const; - - virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings); - -protected slots: - - virtual void updateTopicList(); - -protected: - - // deprecated function for backward compatibility only, use getTopics() instead - ROS_DEPRECATED virtual QList getTopicList(const QSet& message_types, const QList& transports); - - virtual QSet getTopics(const QSet& message_types, const QSet& message_sub_types, const QList& transports); - - virtual void selectTopic(const QString& topic); - -protected slots: - - virtual void onTopicChanged(int index); - - virtual void onZoom1(bool checked); - - virtual void onDynamicRange(bool checked); - - virtual void saveImage(); - -protected: - - virtual void callbackImage(const sensor_msgs::Image::ConstPtr& msg); - - Ui::ImageViewWidget ui_; - - QWidget* widget_; - - image_transport::Subscriber subscriber_; - - cv::Mat conversion_mat_; - -private: - - QString arg_topic_name; - -}; - -} - -#endif // rqt_image_view__ImageView_H diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h b/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h deleted file mode 100644 index dccf9636..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef rqt_image_view__RatioLayoutedFrame_H -#define rqt_image_view__RatioLayoutedFrame_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rqt_image_view { - -/** - * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect ratio. - * The default aspect ratio is 4:3. - */ -class RatioLayoutedFrame - : public QFrame -{ - - Q_OBJECT - -public: - - RatioLayoutedFrame(QWidget* parent, Qt::WFlags flags = 0); - - virtual ~RatioLayoutedFrame(); - - const QImage& getImage() const; - - QImage getImageCopy() const; - - void setImage(const QImage& image); - - QRect getAspectRatioCorrectPaintArea(); - - void resizeToFitAspectRatio(); - - void setInnerFrameMinimumSize(const QSize& size); - - void setInnerFrameMaximumSize(const QSize& size); - - void setInnerFrameFixedSize(const QSize& size); - -signals: - - void delayed_update(); - -protected: - - void setAspectRatio(unsigned short width, unsigned short height); - - void paintEvent(QPaintEvent* event); - -private: - - static int greatestCommonDivisor(int a, int b); - - QSize aspect_ratio_; - - QImage qimage_; - mutable QMutex qimage_mutex_; - -}; - -} - -#endif // rqt_image_view__RatioLayoutedFrame_H diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox deleted file mode 100644 index 93550db9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_image_view provides a GUI plugin for displaying images using image_transport. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/package.xml b/workspace/src/rqt_common_plugins/rqt_image_view/package.xml deleted file mode 100644 index ebc59429..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_image_view - 0.3.13 - rqt_image_view provides a GUI plugin for displaying images using image_transport. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_image_view - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dirk Thomas - - catkin - - cv_bridge - image_transport - rqt_gui - rqt_gui_cpp - sensor_msgs - cv_bridge - image_transport - rqt_gui - rqt_gui_cpp - sensor_msgs - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml b/workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml deleted file mode 100644 index 824cfbf9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A GUI plugin to view a sensor_msgs/Image topic. - - - - - folder - Plugins related to visualization. - - - image-x-generic - A GUI plugin for viewing sensor_msgs/Image topics. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view b/workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view deleted file mode 100755 index b655911a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - - -def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_image_view plugin') - group.add_argument('topic', nargs='?', help='The topic name to subscribe to') - -main = Main() -sys.exit(main.main( - sys.argv, - standalone='rqt_image_view/ImageView', - plugin_argument_provider=add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/setup.py b/workspace/src/rqt_common_plugins/rqt_image_view/setup.py deleted file mode 100644 index ae646dc3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_image_view'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp b/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp deleted file mode 100644 index 8ad95938..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp +++ /dev/null @@ -1,371 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -namespace rqt_image_view { - -ImageView::ImageView() - : rqt_gui_cpp::Plugin() - , widget_(0) -{ - setObjectName("ImageView"); -} - -void ImageView::initPlugin(qt_gui_cpp::PluginContext& context) -{ - widget_ = new QWidget(); - ui_.setupUi(widget_); - - if (context.serialNumber() > 1) - { - widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")"); - } - context.addWidget(widget_); - - updateTopicList(); - ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText("")); - connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int))); - - ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh")); - connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList())); - - ui_.zoom_1_push_button->setIcon(QIcon::fromTheme("zoom-original")); - connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, SLOT(onZoom1(bool))); - - connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool))); - - ui_.save_as_image_push_button->setIcon(QIcon::fromTheme("image-x-generic")); - connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, SLOT(saveImage())); - - // set topic name if passed in as argument - const QStringList& argv = context.argv(); - if (!argv.empty()) { - arg_topic_name = argv[0]; - // add topic name to list if not yet in - int index = ui_.topics_combo_box->findText(arg_topic_name); - if (index == -1) { - QString label(arg_topic_name); - label.replace(" ", "/"); - ui_.topics_combo_box->addItem(label, QVariant(arg_topic_name)); - ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText(arg_topic_name)); - } - } -} - -void ImageView::shutdownPlugin() -{ - subscriber_.shutdown(); -} - -void ImageView::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const -{ - QString topic = ui_.topics_combo_box->currentText(); - //qDebug("ImageView::saveSettings() topic '%s'", topic.toStdString().c_str()); - instance_settings.setValue("topic", topic); - instance_settings.setValue("zoom1", ui_.zoom_1_push_button->isChecked()); - instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked()); - instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value()); -} - -void ImageView::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) -{ - bool zoom1_checked = instance_settings.value("zoom1", false).toBool(); - ui_.zoom_1_push_button->setChecked(zoom1_checked); - - bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool(); - ui_.dynamic_range_check_box->setChecked(dynamic_range_checked); - - double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble(); - ui_.max_range_double_spin_box->setValue(max_range); - - QString topic = instance_settings.value("topic", "").toString(); - // don't overwrite topic name passed as command line argument - if (!arg_topic_name.isEmpty()) - { - arg_topic_name = ""; - } - else - { - //qDebug("ImageView::restoreSettings() topic '%s'", topic.toStdString().c_str()); - selectTopic(topic); - } -} - -void ImageView::updateTopicList() -{ - QSet message_types; - message_types.insert("sensor_msgs/Image"); - QSet message_sub_types; - message_sub_types.insert("sensor_msgs/CompressedImage"); - - // get declared transports - QList transports; - image_transport::ImageTransport it(getNodeHandle()); - std::vector declared = it.getDeclaredTransports(); - for (std::vector::const_iterator it = declared.begin(); it != declared.end(); it++) - { - //qDebug("ImageView::updateTopicList() declared transport '%s'", it->c_str()); - QString transport = it->c_str(); - - // strip prefix from transport name - QString prefix = "image_transport/"; - if (transport.startsWith(prefix)) - { - transport = transport.mid(prefix.length()); - } - transports.append(transport); - } - - QString selected = ui_.topics_combo_box->currentText(); - - // fill combo box - QList topics = getTopics(message_types, message_sub_types, transports).values(); - topics.append(""); - qSort(topics); - ui_.topics_combo_box->clear(); - for (QList::const_iterator it = topics.begin(); it != topics.end(); it++) - { - QString label(*it); - label.replace(" ", "/"); - ui_.topics_combo_box->addItem(label, QVariant(*it)); - } - - // restore previous selection - selectTopic(selected); -} - -QList ImageView::getTopicList(const QSet& message_types, const QList& transports) -{ - QSet message_sub_types; - return getTopics(message_types, message_sub_types, transports).values(); -} - -QSet ImageView::getTopics(const QSet& message_types, const QSet& message_sub_types, const QList& transports) -{ - ros::master::V_TopicInfo topic_info; - ros::master::getTopics(topic_info); - - QSet all_topics; - for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++) - { - all_topics.insert(it->name.c_str()); - } - - QSet topics; - for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++) - { - if (message_types.contains(it->datatype.c_str())) - { - QString topic = it->name.c_str(); - - // add raw topic - topics.insert(topic); - //qDebug("ImageView::getTopics() raw topic '%s'", topic.toStdString().c_str()); - - // add transport specific sub-topics - for (QList::const_iterator jt = transports.begin(); jt != transports.end(); jt++) - { - if (all_topics.contains(topic + "/" + *jt)) - { - QString sub = topic + " " + *jt; - topics.insert(sub); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", sub.toStdString().c_str()); - } - } - } - if (message_sub_types.contains(it->datatype.c_str())) - { - QString topic = it->name.c_str(); - int index = topic.lastIndexOf("/"); - if (index != -1) - { - topic.replace(index, 1, " "); - topics.insert(topic); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", topic.toStdString().c_str()); - } - } - } - return topics; -} - -void ImageView::selectTopic(const QString& topic) -{ - int index = ui_.topics_combo_box->findText(topic); - if (index == -1) - { - index = ui_.topics_combo_box->findText(""); - } - ui_.topics_combo_box->setCurrentIndex(index); -} - -void ImageView::onTopicChanged(int index) -{ - subscriber_.shutdown(); - - // reset image on topic change - ui_.image_frame->setImage(QImage()); - - QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" "); - QString topic = parts.first(); - QString transport = parts.length() == 2 ? parts.last() : "raw"; - - if (!topic.isEmpty()) - { - image_transport::ImageTransport it(getNodeHandle()); - image_transport::TransportHints hints(transport.toStdString()); - try { - subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, hints); - //qDebug("ImageView::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), subscriber_.getTransport().c_str()); - } catch (image_transport::TransportLoadException& e) { - QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what()); - } - } -} - -void ImageView::onZoom1(bool checked) -{ - if (checked) - { - if (ui_.image_frame->getImage().isNull()) - { - return; - } - ui_.image_frame->setInnerFrameFixedSize(ui_.image_frame->getImage().size()); - widget_->resize(ui_.image_frame->size()); - widget_->setMinimumSize(widget_->sizeHint()); - widget_->setMaximumSize(widget_->sizeHint()); - } else { - ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60)); - ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); - widget_->setMinimumSize(QSize(80, 60)); - widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); - } -} - -void ImageView::onDynamicRange(bool checked) -{ - ui_.max_range_double_spin_box->setEnabled(!checked); -} - -void ImageView::saveImage() -{ - // take a snapshot before asking for the filename - QImage img = ui_.image_frame->getImageCopy(); - - QString file_name = QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", tr("Image (*.bmp *.jpg *.png *.tiff)")); - if (file_name.isEmpty()) - { - return; - } - - img.save(file_name); -} - -void ImageView::callbackImage(const sensor_msgs::Image::ConstPtr& msg) -{ - try - { - // First let cv_bridge do its magic - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); - conversion_mat_ = cv_ptr->image; - } - catch (cv_bridge::Exception& e) - { - try - { - // If we're here, there is no conversion that makes sense, but let's try to imagine a few first - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg); - if (msg->encoding == "CV_8UC3") - { - // assuming it is rgb - conversion_mat_ = cv_ptr->image; - } else if (msg->encoding == "8UC1") { - // convert gray to rgb - cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB); - } else if (msg->encoding == "16UC1" || msg->encoding == "32FC1") { - // scale / quantify - double min = 0; - double max = ui_.max_range_double_spin_box->value(); - if (msg->encoding == "16UC1") max *= 1000; - if (ui_.dynamic_range_check_box->isChecked()) - { - // dynamically adjust range based on min/max in image - cv::minMaxLoc(cv_ptr->image, &min, &max); - if (min == max) { - // completely homogeneous images are displayed in gray - min = 0; - max = 2; - } - } - cv::Mat img_scaled_8u; - cv::Mat(cv_ptr->image-min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min)); - cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB); - } else { - qWarning("ImageView.callback_image() could not convert image from '%s' to 'rgb8' (%s)", msg->encoding.c_str(), e.what()); - ui_.image_frame->setImage(QImage()); - return; - } - } - catch (cv_bridge::Exception& e) - { - qWarning("ImageView.callback_image() while trying to convert image from '%s' to 'rgb8' an exception was thrown (%s)", msg->encoding.c_str(), e.what()); - ui_.image_frame->setImage(QImage()); - return; - } - } - - // image must be copied since it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation - QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, conversion_mat_.step[0], QImage::Format_RGB888); - ui_.image_frame->setImage(image); - - if (!ui_.zoom_1_push_button->isEnabled()) - { - ui_.zoom_1_push_button->setEnabled(true); - onZoom1(ui_.zoom_1_push_button->isChecked()); - } -} - -} - -PLUGINLIB_EXPORT_CLASS(rqt_image_view::ImageView, rqt_gui_cpp::Plugin) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui b/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui deleted file mode 100644 index bad42ee9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui +++ /dev/null @@ -1,169 +0,0 @@ - - - ImageViewWidget - - - true - - - - 0 - 0 - 400 - 300 - - - - Image View - - - - - - - - QComboBox::AdjustToContents - - - - - - - - - - false - - - true - - - - - - - Dynamic depth range - - - - - - - - - - Max depth - - - m - - - 0.010000000000000 - - - 100.000000000000000 - - - 10.000000000000000 - - - - - - - Save as image - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - 0 - - - - - - 0 - 0 - - - - - 80 - 60 - - - - QFrame::Box - - - 1 - - - - - - - Qt::Horizontal - - - QSizePolicy::Expanding - - - - 0 - 20 - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - rqt_image_view::RatioLayoutedFrame - QFrame -
rqt_image_view/ratio_layouted_frame.h
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp b/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp deleted file mode 100644 index bf608613..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include - -namespace rqt_image_view { - -RatioLayoutedFrame::RatioLayoutedFrame(QWidget* parent, Qt::WFlags flags) - : QFrame() - , aspect_ratio_(4, 3) -{ - connect(this, SIGNAL(delayed_update()), this, SLOT(update()), Qt::QueuedConnection); -} - -RatioLayoutedFrame::~RatioLayoutedFrame() -{ -} - -const QImage& RatioLayoutedFrame::getImage() const -{ - return qimage_; -} - -QImage RatioLayoutedFrame::getImageCopy() const -{ - QImage img; - qimage_mutex_.lock(); - img = qimage_.copy(); - qimage_mutex_.unlock(); - return img; -} - -void RatioLayoutedFrame::setImage(const QImage& image)//, QMutex* image_mutex) -{ - qimage_mutex_.lock(); - qimage_ = image.copy(); - qimage_mutex_.unlock(); - setAspectRatio(qimage_.width(), qimage_.height()); - emit delayed_update(); -} - -void RatioLayoutedFrame::resizeToFitAspectRatio() -{ - QRect rect = contentsRect(); - - // reduce longer edge to aspect ration - double width = double(rect.width()); - double height = double(rect.height()); - if (width * aspect_ratio_.height() / height > aspect_ratio_.width()) - { - // too large width - width = height * aspect_ratio_.width() / aspect_ratio_.height(); - rect.setWidth(int(width + 0.5)); - } - else - { - // too large height - height = width * aspect_ratio_.height() / aspect_ratio_.width(); - rect.setHeight(int(height + 0.5)); - } - - // resize taking the border line into account - int border = lineWidth(); - resize(rect.width() + 2 * border, rect.height() + 2 * border); -} - -void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize& size) -{ - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMinimumSize(new_size); - emit delayed_update(); -} - -void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize& size) -{ - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMaximumSize(new_size); - emit delayed_update(); -} - -void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize& size) -{ - setInnerFrameMinimumSize(size); - setInnerFrameMaximumSize(size); -} - -void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) -{ - int divisor = greatestCommonDivisor(width, height); - if (divisor != 0) { - aspect_ratio_.setWidth(width / divisor); - aspect_ratio_.setHeight(height / divisor); - } -} - -void RatioLayoutedFrame::paintEvent(QPaintEvent* event) -{ - QPainter painter(this); - qimage_mutex_.lock(); - if (!qimage_.isNull()) - { - resizeToFitAspectRatio(); - // TODO: check if full draw is really necessary - //QPaintEvent* paint_event = dynamic_cast(event); - //painter.drawImage(paint_event->rect(), qimage_); - painter.drawImage(contentsRect(), qimage_); - } else { - // default image with gradient - QLinearGradient gradient(0, 0, frameRect().width(), frameRect().height()); - gradient.setColorAt(0, Qt::white); - gradient.setColorAt(1, Qt::black); - painter.setBrush(gradient); - painter.drawRect(0, 0, frameRect().width() + 1, frameRect().height() + 1); - } - qimage_mutex_.unlock(); -} - -int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) -{ - if (b==0) - { - return a; - } - return greatestCommonDivisor(b, a % b); -} - -} diff --git a/workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst deleted file mode 100644 index e2f9dc59..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst +++ /dev/null @@ -1,75 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_launch -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* mark rqt_launch and rqt_reconfigure as experimental (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* add stop-all-nodes button - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- -* Fix; Start-Stop buttons now reflect nodes status, except when they crashed - -0.2.13 (2013-03-11 22:14) -------------------------- -* Nodes are stop-able now. -* Capable to add node widgets as children of nodes. - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt deleted file mode 100644 index a47cf796..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_launch) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_launch - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/package.xml b/workspace/src/rqt_common_plugins/rqt_launch/package.xml deleted file mode 100644 index 0d73bed3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - rqt_launch - 0.3.13 - This rqt plugin ROS package provides easy view of .launch files. - User can also start and end node by node that are defined in those files. - - - Isaac Saito - BSD - - http://ros.org/wiki/rqt_launch - https://github.com/ros-visualization/rqt_common_plugins/rqt_launch - https://github.com/ros-visualization/rqt_common_plugins/issues - - Isaac Saito - Stuart Glaser - - catkin - rqt_py_common - roslaunch - rospy - rqt_console - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/plugin.xml b/workspace/src/rqt_common_plugins/rqt_launch/plugin.xml deleted file mode 100644 index 9536e0e9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/plugin.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - A Python GUI plugin for for edit launch file, run and stop nodes. - - - - - folder - Plugins related to configuration. - - - mail-message-new - A Python GUI plugin for editing, rnuning and stopping launch files. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui b/workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui deleted file mode 100644 index e6e5fffc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui +++ /dev/null @@ -1,77 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 761 - 484 - - - - - 300 - 30 - - - - ROS Launch GUI - - - - - - - - - - - - - - S&TART ALL - - - - - - - false - - - STO&P ALL - - - - - - - (Not implemented yet) - - - &Open Console - - - - - - - - - true - - - true - - - true - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui b/workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui deleted file mode 100644 index 2dcc29b3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui +++ /dev/null @@ -1,108 +0,0 @@ - - - NodeWidget - - - - 0 - 0 - 764 - 57 - - - - - 300 - 30 - - - - ROS Launch GUI - - - - - - - - Node name - - - &Status - - - - - - - #spawned so far - - - (0) - - - - - - - Start node - - - - - - - - true - - - - - - - Reload - - - - - - - - true - - - - - - - Pkg name - - - Package name - - - - - - - Executable name - - - Executable - - - - - - - (argument) - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch b/workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch deleted file mode 100755 index 55f94f40..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_launch.launch_plugin.LaunchPlugin')) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/setup.py b/workspace/src/rqt_common_plugins/rqt_launch/setup.py deleted file mode 100644 index add6e5ea..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_launch'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/__init__.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py deleted file mode 100644 index 192baafa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py +++ /dev/null @@ -1,154 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import sys - -import rospy -from rqt_launch.launch_widget import LaunchWidget -from rqt_py_common.plugin_container_widget import PluginContainerWidget -from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil - -import re - - - -def is_int(s): - try: - int(s) - return True - except ValueError: - return False - - -def is_float(s): - try: - float(s) - return True - except ValueError: - return False - - -class LaunchMain(object): - def __init__(self, plugin_context): - super(LaunchMain, self).__init__() - print 'sfdfdsfsdfds' - self._plugin_context = plugin_context - - self._main_launch_widget = LaunchWidget(self) - self._mainwidget = PluginContainerWidget(self._main_launch_widget, - True, False) - - self._run_id = None - self._node_controllers = [] - self._config = None - - #RqtRoscommUtil.load_parameters(self._config, '/rqt_launch') - - def get_widget(self): - return self._mainwidget - - def set_node_controllers(self, node_controllers): - self._node_controllers = node_controllers - - def set_config(self, config): - self._config = config - - def start_all(self): - ''' - Checks nodes that's set (via self.set_node_controllers) one by one and - starts one if each node is not running. - Then disable START ALL button and enable STOP ALL button. - ''' - - pat = re.compile(r'.*=(.*)') - if self._config: - for k in self._config.params.keys(): - s = str(self._config.params[k]) - result = re.match(pat, s) - - if result: - val = result.groups()[0] - - if is_int(val): - rospy.set_param(k, int(val)) - elif is_float(val): - rospy.set_param(k, float(val)) - else: - rospy.set_param(k, val) - - - for n in self._node_controllers: - if not n.is_node_running(): - n.start(restart=False) - - # Disable START ALL button. - self._main_launch_widget._pushbutton_start_all.setEnabled(False) - self._main_launch_widget._pushbutton_stop_all.setEnabled(True) - - def stop_all(self): - ''' - Checks nodes that's set (via self.set_node_controllers) one by one and - stops one if each node is running. - Then enable START ALL button and disable STOP ALL button. - ''' - - for n in self._node_controllers: - if n.is_node_running(): - n.stop() - - # Disable STOP ALL button. - self._main_launch_widget._pushbutton_start_all.setEnabled(True) - self._main_launch_widget._pushbutton_stop_all.setEnabled(False) - - def check_process_statuses(self): - for n in self._node_controllers: - n.check_process_status() - - def shutdown(self): - rospy.logdebug('Launchmain.shutdown') - self.stop_all() - - def save_settings(self, plugin_settings, instance_settings): - self._mainwidget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._mainwidget.restore_settings(plugin_settings, instance_settings) - - -if __name__ == '__main__': - # main should be used only for debug purpose. - # This launches this QWidget as a standalone rqt gui. - from rqt_gui.main import Main - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_launch')) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py deleted file mode 100644 index 5aed4801..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py +++ /dev/null @@ -1,64 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from rqt_gui_py.plugin import Plugin - -from rqt_launch.launch_main import LaunchMain - - -class LaunchPlugin(Plugin): - - def __init__(self, context): - ''' - :type context: qt_gui.PluginContext - ''' - - super(LaunchPlugin, self).__init__(context) - - self._main = LaunchMain(context) - - self._widget = self._main.get_widget() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._main.shutdown() - - def save_settings(self, plugin_settings, instance_settings): - self._main.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._main.restore_settings(plugin_settings, instance_settings) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py deleted file mode 100644 index c96be051..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py +++ /dev/null @@ -1,274 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import os -import sys - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QModelIndex, Signal -from python_qt_binding.QtGui import QDialog, QStandardItem, \ - QStandardItemModel -from rosgraph import rosenv -import roslaunch -from roslaunch.core import RLException -import rospkg -import rospy - -#from rqt_console.console_widget import ConsoleWidget -from rqt_launch.node_proxy import NodeProxy -from rqt_launch.node_controller import NodeController -from rqt_launch.node_delegate import NodeDelegate -from rqt_launch.status_indicator import StatusIndicator -from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil - - -class LaunchWidget(QDialog): - '''#TODO: comment - ''' - - # To be connected to PluginContainerWidget - sig_sysmsg = Signal(str) - - def __init__(self, parent): - ''' - @type parent: LaunchMain - ''' - super(LaunchWidget, self).__init__() - self._parent = parent - print 'FROM WIDGET' - - self._config = None - - #TODO: should be configurable from gui - self._port_roscore = 11311 - - self._run_id = None - - self._rospack = rospkg.RosPack() - ui_file = os.path.join(self._rospack.get_path('rqt_launch'), - 'resource', 'launch_widget.ui') - loadUi(ui_file, self) - - # row=0 allows any number of rows to be added. - self._datamodel = QStandardItemModel(0, 1) - - master_uri = rosenv.get_master_uri() - rospy.loginfo('LaunchWidget master_uri={}'.format(master_uri)) - self._delegate = NodeDelegate(master_uri, - self._rospack) - self._treeview.setModel(self._datamodel) - self._treeview.setItemDelegate(self._delegate) - - # NodeController used in controller class for launch operation. - self._node_controllers = [] - - self._pushbutton_start_all.clicked.connect(self._parent.start_all) - self._pushbutton_stop_all.clicked.connect(self._parent.stop_all) - # Bind package selection with .launch file selection. - self._combobox_pkg.currentIndexChanged[str].connect( - self._refresh_launchfiles) - # Bind a launch file selection with launch GUI generation. - self._combobox_launchfile_name.currentIndexChanged[str].connect( - self._load_launchfile_slot) - self._update_pkgs_contain_launchfiles() - - # Used for removing previous nodes - self._num_nodes_previous = 0 - - def _load_launchfile_slot(self, launchfile_name): - # Not yet sure why, but everytime combobox.currentIndexChanged occurs, - # this method gets called twice with launchfile_name=='' in 1st call. - if launchfile_name == None or launchfile_name == '': - return - - _config = None - - rospy.logdebug('_load_launchfile_slot launchfile_name={}'.format( - launchfile_name)) - - try: - _config = self._create_launchconfig(launchfile_name, - self._port_roscore) - #TODO: folder_name_launchfile should be able to specify arbitrarily - # _create_launchconfig takes 3rd arg for it. - - except IndexError as e: - msg = 'IndexError={} launchfile={}'.format(e.message, - launchfile_name) - rospy.logerr(msg) - self.sig_sysmsg.emit(msg) - return - except RLException as e: - msg = 'RLException={} launchfile={}'.format(e.message, - launchfile_name) - rospy.logerr(msg) - self.sig_sysmsg.emit(msg) - return - - self._create_widgets_for_launchfile(_config) - - def _create_launchconfig(self, launchfile_name, port_roscore=11311, - folder_name_launchfile='launch'): - ''' - @rtype: ROSLaunchConfig - @raises RLException: raised by roslaunch.config.load_config_default. - ''' - - pkg_name = self._combobox_pkg.currentText() - - try: - launchfile = os.path.join(self._rospack.get_path(pkg_name), - folder_name_launchfile, launchfile_name) - except IndexError as e: - raise RLException('IndexError: {}'.format(e.message)) - - try: - launch_config = roslaunch.config.load_config_default([launchfile], - port_roscore) - except rospkg.common.ResourceNotFound as e: - raise RLException('ResourceNotFound: {}'.format(e.message)) - except RLException as e: - raise e - - return launch_config - - def _create_widgets_for_launchfile(self, config): - self._config = config - - # Delete old nodes' GUIs. - self._node_controllers = [] - - # These lines seem to remove indexWidgets previously set on treeview. - # Per suggestion in API doc, we are not using reset(). Instead, - # using 2 methods below without any operation in between, which - # I suspect might be inproper. - # http://qt-project.org/doc/qt-4.8/qabstractitemmodel.html#reset - self._datamodel.beginResetModel() - self._datamodel.endResetModel() - - # Need to store the num of nodes outside of the loop -- this will - # be used for removing excessive previous node rows. - order_xmlelement = 0 - # Loop per xml element - for order_xmlelement, node in enumerate(self._config.nodes): - proxy = NodeProxy(self._run_id, self._config.master.uri, node) - - # TODO: consider using QIcon.fromTheme() - status_label = StatusIndicator() - - qindex_nodewidget = self._datamodel.index(order_xmlelement, - 0, QModelIndex()) - node_widget = self._delegate.create_node_widget(qindex_nodewidget, - proxy.config, - status_label) - - #TODO: Ideally find a way so that we don't need this block. - #BEGIN If these lines are missing, widget won't be shown either. - std_item = QStandardItem( - #node_widget.get_node_name() - ) - self._datamodel.setItem(order_xmlelement, 0, std_item) - #END If these lines are missing, widget won't be shown either. - - self._treeview.setIndexWidget(qindex_nodewidget, node_widget) - - node_controller = NodeController(proxy, node_widget) - self._node_controllers.append(node_controller) - - node_widget.connect_start_stop_button( \ - node_controller.start_stop_slot) - rospy.logdebug('loop #%d proxy.config.namespace=%s ' + - 'proxy.config.name=%s', - order_xmlelement, proxy.config.namespace, - proxy.config.name) - - self._num_nodes_previous = order_xmlelement - - self._parent.set_node_controllers(self._node_controllers) - self._parent.set_config(config) - - def _update_pkgs_contain_launchfiles(self): - ''' - Inspired by rqt_msg.MessageWidget._update_pkgs_contain_launchfiles - ''' - packages = sorted([pkg_tuple[0] - for pkg_tuple - in RqtRoscommUtil.iterate_packages('launch')]) - self._package_list = packages - rospy.logdebug('pkgs={}'.format(self._package_list)) - self._combobox_pkg.clear() - self._combobox_pkg.addItems(self._package_list) - self._combobox_pkg.setCurrentIndex(0) - - def _refresh_launchfiles(self, package=None): - ''' - Inspired by rqt_msg.MessageWidget._refresh_msgs - ''' - if package is None or len(package) == 0: - return - self._launchfile_instances = [] # Launch[] - #TODO: RqtRoscommUtil.list_files's 2nd arg 'subdir' should NOT be - # hardcoded later. - _launch_instance_list = RqtRoscommUtil.list_files(package, - 'launch') - - rospy.logdebug('_refresh_launches package={} instance_list={}'.format( - package, - _launch_instance_list)) - - self._launchfile_instances = [x.split('/')[1] - for x in _launch_instance_list] - - self._combobox_launchfile_name.clear() - self._combobox_launchfile_name.addItems(self._launchfile_instances) - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value('splitter', self._splitter.saveState()) - pass - - def restore_settings(self, plugin_settings, instance_settings): -# if instance_settings.contains('splitter'): -# self._splitter.restoreState(instance_settings.value('splitter')) -# else: -# self._splitter.setSizes([100, 100, 200]) - pass - - -if __name__ == '__main__': - # main should be used only for debug purpose. - # This launches this QWidget as a standalone rqt gui. - from rqt_gui.main import Main - - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_launch')) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py deleted file mode 100644 index 241c3cd4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py +++ /dev/null @@ -1,61 +0,0 @@ -#! /usr/bin/env python - - -class NamesSurrogate(object): - ''' - Because some functions in roslib.names cannot be referred in the original - rxlaunch code, the codes of those function are copied here. This class - should not be used for any other purpose than to be used within this .py - file. - - :author: Isaac Saito - ''' - - PRIV_NAME = '~' - SEP = '/' - - @staticmethod - def is_global(name): - ''' - Test if name is a global graph resource name. 116 117 - @param name: must be a legal name in canonical form 118 - @type name: str 119 - @return: True if name is a globally referenced name (i.e. /ns/name) 120 - @rtype: bool - ''' - return name and name[0] == NamesSurrogate.SEP - - @staticmethod - def is_private(name): - ''' 126 Test if name is a private graph resource name. 127 128 - @param name: must be a legal name in canonical form 129 - @type name: str 130 @return bool: True if name is a privately - referenced name (i.e. ~name) 131 ''' - return name and name[0] == NamesSurrogate.PRIV_NAME - - @staticmethod - def ns_join(ns, name): - ''' - Taken from - http://ros.org/rosdoclite/groovy/api/roslib/html/python/roslib.names-pysrc.html#ns_join - since roslib.names is not found for some reason, and also the entire - module seems deprecated. - - Join a namespace and name. If name is unjoinable (i.e. ~private or - 162 /global) it will be returned without joining 163 164 - @param ns: namespace ('/' and '~' are both legal). If ns is the empty - string, name will be returned. 165 - @type ns: str 166 - @param name str: a legal name 167 - @return str: name concatenated to ns, or name if it's 168 unjoinable. 169 - @rtype: str 170 - ''' - if NamesSurrogate.is_private(name) or NamesSurrogate.is_global(name): - return name - if ns == NamesSurrogate.PRIV_NAME: - return NamesSurrogate.PRIV_NAME + name - if not ns: - return name - if ns[-1] == NamesSurrogate.SEP: - return ns + name - return ns + NamesSurrogate.SEP + name diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py deleted file mode 100644 index fbd860b4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py +++ /dev/null @@ -1,116 +0,0 @@ -#! /usr/bin/env python - -import rospy - - -# Provides callback functions for the start and stop buttons -class NodeController(object): - ''' - Containing both proxy and gui instances, this class gives a control of - a node on both ROS & GUI sides. - ''' - - # these values need to synch with member variables. - # Eg. self.gui isn't legal. - __slots__ = ['_proxy', '_gui'] - - def __init__(self, proxy, gui): - ''' - @type proxy: rqt_launch.NodeProxy - @type gui: QWidget - ''' - self._proxy = proxy - - self._gui = gui - self._gui.set_node_controller(self) - - def start_stop_slot(self, signal): - ''' - Works as a slot particularly intended to work for - QAbstractButton::toggled(checked). Internally calls - NodeController.start / stop depending on `signal`. - - @type signal: bool - ''' - if self._proxy.is_running(): - self.stop() - rospy.logdebug('---start_stop_slot stOP') - else: - self.start() - rospy.logdebug('==start_stop_slot StART') - - def start(self, restart=True): - ''' - Start a ROS node as a new _process. - ''' - rospy.logdebug('Controller.start restart={}'.format(restart)) - - # Should be almost unreachable under current design where this 'start' - # method only gets called when _proxy.is_running() returns false. - - if self._proxy.is_running(): - if not restart: - # If the node is already running and restart isn't desired, - # do nothing further. - return - #TODO: Need to consider...is stopping node here - # (i.e. in 'start' method) good? - self.stop() - - # If the launch_prefix has changed, then the _process must be recreated - if (self._proxy.config.launch_prefix != - self._gui._lineEdit_launch_args.text()): - - self._proxy.config.launch_prefix = \ - self._gui._lineEdit_launch_args.text() - self._proxy.recreate_process() - - self._gui.set_node_started(False) - self._gui.label_status.set_starting() - self._proxy.start_process() - self._gui.label_status.set_running() - self._gui.label_spawncount.setText("({})".format( - self._proxy.get_spawn_count())) - - def stop(self): - ''' - Stop a ROS node's _process. - ''' - - #TODO: Need to check if the node is really running. - - #if self._proxy.is_running(): - self._gui.set_node_started(True) - self._gui.label_status.set_stopping() - self._proxy.stop_process() - self._gui.label_status.set_stopped() - - def check_process_status(self): - if self._proxy.has_died(): - rospy.logerr("Process died: {}".format( - self._proxy.get_proc_name())) - self._proxy.stop_process() - self._gui.set_node_started(True) - if self._proxy._process.exit_code == 0: - self._gui.label_status.set_stopped() - else: - self._gui.label_status.set_died() - - # Checks if it should be respawned - if self._gui.respawn_toggle.isChecked(): - rospy.loginfo("Respawning _process: {}".format( - self._proxy._process.name)) - self._gui.label_status.set_starting() - self._proxy.start_process() - self._gui.label_status.set_running() - self._gui.label_spawncount.setText("({})".format( - self._proxy._process.spawn_count)) - - def get_node_widget(self): - ''' - @rtype: QWidget - ''' - return self._gui - - def is_node_running(self): - return self._proxy.is_running() diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py deleted file mode 100644 index 3e548efc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py +++ /dev/null @@ -1,93 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QStyledItemDelegate -import rospkg - -from rqt_launch.node_widget import NodeWidget - - -class NodeDelegate(QStyledItemDelegate): - __slots__ = ['status_label', 'respawn_toggle', 'spawn_count_label', - 'launch_prefix_edit'] - - def __init__(self, master_uri, rospack=None): - #respawn_toggle, spawn_count_label, launch_prefix_edit): - super(NodeDelegate, self).__init__() - self._master_uri = master_uri - - self._rospack = rospack - if rospack == None: - self._rospack = rospkg.RosPack() - - self._nodewidget_dict = {} # { QModelIndex : QWidget } - - def createEditor(self, parent, option, index): - '''Overridden''' - - nodewidget = self._nodewidget_dict[index] - #TODO: handle exception - return nodewidget - - def setEditorData(self, spinBox, index): - '''Overridden''' - value = index.model().data(index, Qt.EditRole) - - spinBox.setValue(value) - - def setModelData(self, spinBox, model, index): - '''Overridden''' - spinBox.interpretText() - value = spinBox.value() - - model.setData(index, value, Qt.EditRole) - - def updateEditorGeometry(self, editor, option, index): - '''Overridden''' - editor.setGeometry(option.rect) - - def create_node_widget(self, qindex, launch_config, - status_label): - ''' - @type status_label: StatusIndicator - ''' - nodewidget = NodeWidget(self._rospack, - self._master_uri, launch_config, - status_label) - self._nodewidget_dict[qindex] = nodewidget - return nodewidget - - def get_node_widget(self): - return self._node_widget diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py deleted file mode 100644 index 91d92a37..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py +++ /dev/null @@ -1,125 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import time -import threading - -from roslaunch import node_args, nodeprocess -import rospy - - -class Polling(threading.Thread): - def __init__(self, parent): - super(Polling, self).__init__() - self._parent = parent - - def run(self): - while True: - rospy.logdebug('Proc={} Died? {}'.format( - self._parent.get_proc_name(), - self._parent.has_died())) - time.sleep(1.0) - - -class NodeProxy(object): - ''' - Works as a proxy between ROS Node - (more in particular, roslaunch.nodeprocess) & GUI. - ''' - - __slots__ = ['_run_id', 'master_uri', 'config', '_process'] - - def __init__(self, run_id, master_uri, config): - self._run_id = run_id - self.master_uri = master_uri - self.config = config - - # @type: roslaunch.nodeprocess.LocalProcess - self._process = self.recreate_process() - - # LocalProcess.is_alive() does not do what you would expect - def is_running(self): - rospy.logdebug('BEFORE started={}, stopped={} alive={}'.format( - self._process.started, - self._process.stopped, - self._process.is_alive())) - return self._process.started and not self._process.stopped - #and self._process.is_alive() - # is_alive() returns False once nodeprocess was stopped. - - def has_died(self): - rospy.logdebug('Proc={} started={}, stopped={}, is_alive={}'.format( - self.get_proc_name(), self._process.started, self._process.stopped, - self._process.is_alive())) - - return (self._process.started and not self._process.stopped - and not self._process.is_alive()) - - def recreate_process(self): - ''' - Create and set roslaunch.nodeprocess.LocalProcess to member variable. - @rtype: roslaunch.nodeprocess.LocalProcess - ''' - _local_process = nodeprocess.LocalProcess - try: - _local_process = nodeprocess.create_node_process( - self._run_id, self.config, self.master_uri) - except node_args.NodeParamsException as e: - rospy.logerr('roslaunch failed to load the node. {}'.format( - e.message)) - #TODO: Show msg on GUI that the node wasn't loaded. - - return _local_process - - def start_process(self): - #TODO: add null exception check for _process - self._process.start() - - def stop_process(self): - #TODO: add null exception check for _process - - try: - self._process.stop() - except Exception as e: - #TODO: Change roslaunch to raise exception - rospy.logdebug('Proxy stop_process exception={}'.format(e.message)) - - def get_spawn_count(self): - return self._process.spawn_count - - def get_proc_name(self): - return self._process.name - - def get_proc_exit_code(self): - return self._process.exit_code diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py deleted file mode 100644 index c6ddcbf1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py +++ /dev/null @@ -1,119 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QIcon, QLineEdit, QWidget -import rospy - -from rqt_launch.name_surrogate import NamesSurrogate - - -class NodeWidget(QWidget): - ''' - Works as a proxy between ROS Node - (more in particular, roslaunch.nodeprocess) and GUI. - ''' - - __slots__ = ['_run_id', 'master_uri', 'config', '_process'] - - def __init__(self, rospack, master_uri, launch_config, - label_status): - ''' - @type launch_node: roslaunch.core.Node - @type launch_config: roslaunch.core.Config - @type label_status: StatusIndicator - ''' - super(NodeWidget, self).__init__() - self._rospack = rospack - self._master_uri = master_uri - self._launch_config = launch_config - - ui_file = os.path.join(self._rospack.get_path('rqt_launch'), - 'resource', 'node_widget.ui') - loadUi(ui_file, self) - - self.label_status = label_status # Public - # stop_button = QPushButton(self.style().standardIcon( - # QStyle.SP_MediaStop), "") - self._respawn_toggle.setChecked(self._launch_config.respawn) - self._lineEdit_launch_args = QLineEdit( - self._launch_config.launch_prefix) - - rospy.logdebug('_proxy.conf.namespace={} launch_config={}'.format( - self._launch_config.namespace, self._launch_config.name)) - self._resolved_node_name = NamesSurrogate.ns_join( - self._launch_config.namespace, self._launch_config.name) - self._label_nodename.setText(self._get_node_name()) - self._label_pkg_name.setText(self._launch_config.package) - self._label_name_executable.setText(self._launch_config.type) - - self._icon_node_start = QIcon.fromTheme('media-playback-start') - self._icon_node_stop = QIcon.fromTheme('media-playback-stop') - self._icon_respawn_toggle = QIcon.fromTheme('view-refresh') - - self._pushbutton_start_stop_node.setIcon(self._icon_node_start) - self._respawn_toggle.setIcon(self._icon_respawn_toggle) - - self._node_controller = None # Connected in self.set_node_controller - - def _get_node_name(self): - return self._resolved_node_name - - def connect_start_stop_button(self, slot): - self._pushbutton_start_stop_node.toggled.connect(slot) - - def set_node_started(self, is_started=True): - # If the button is not down yet - is_node_running = self._node_controller.is_node_running() - rospy.logdebug('NodeWidget.set_node_started running?={}'.format( - is_node_running)) - #if is_node_running: - if is_started: - #and self._pushbutton_start_stop_node.isDown(): - self._pushbutton_start_stop_node.setIcon(self._icon_node_start) - self._pushbutton_start_stop_node.setDown(False) - - #elif not is_node_running: # To START the node - else: - #and not self._pushbutton_start_stop_node.isDown(): - self._pushbutton_start_stop_node.setIcon(self._icon_node_stop) - self._pushbutton_start_stop_node.setDown(True) - - def set_node_controller(self, node_controller): - #TODO: Null check - self._node_controller = node_controller - #self._pushbutton_start_stop_node.pressed.connect( - # self._node_controller.start_stop_slot) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py deleted file mode 100644 index 2421e2be..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py +++ /dev/null @@ -1,40 +0,0 @@ -#! /usr/bin/env python - -from python_qt_binding.QtGui import QLabel, QStyle -import rospy - - -class StatusIndicator(QLabel): - def __init__(self, *args): - super(StatusIndicator, self).__init__(*args) - self.set_stopped() - - def set_running(self): - self.setPixmap( - self.style().standardIcon(QStyle.SP_DialogApplyButton).pixmap(16)) - - def set_starting(self): - rospy.logdebug('StatusIndicator.set_starting') - self.setPixmap(self.style().standardIcon( - QStyle.SP_DialogResetButton).pixmap(16)) - - def set_stopping(self): - """ - Show msg that the process is "stopping". - - cf. set_stopped() - """ - self.setPixmap(self.style().standardIcon( - QStyle.SP_DialogResetButton).pixmap(16)) - - def set_stopped(self): - """ - Show msg that the process is "stopped". - - cf. set_stopping() - """ - self.setText(" ") - - def set_died(self): - self.setPixmap(self.style().standardIcon( - QStyle.SP_MessageBoxCritical).pixmap(16)) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/test/test.launch b/workspace/src/rqt_common_plugins/rqt_launch/test/test.launch deleted file mode 100644 index 0ebae4b2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/test/test.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst deleted file mode 100644 index de15ff5d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_logger_level -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt deleted file mode 100644 index 0ed180dd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_logger_level) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_logger_level - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox deleted file mode 100644 index 33481e79..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_logger_level provides a GUI plugin for configuring the logger level of ROS nodes. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/package.xml b/workspace/src/rqt_common_plugins/rqt_logger_level/package.xml deleted file mode 100644 index 8c6662dc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - rqt_logger_level - 0.3.13 - rqt_logger_level provides a GUI plugin for configuring the logger level of ROS nodes.
-
- rqt_logger_level takes over `wx`-based tool [[rxloggerlevel]]. - - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_logger_level - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - python-rospkg - rosnode - rosservice - rospy - rqt_gui - rqt_gui_py - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml b/workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml deleted file mode 100644 index 7bfb4045..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin configuring the logger level of ROS nodes. - - - - - folder - Plugins related to logging. - - - format-indent-more - A Python GUI plugin for configuring the level of ROS loggers. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui b/workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui deleted file mode 100644 index b49274cd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui +++ /dev/null @@ -1,74 +0,0 @@ - - - LoggerLevel - - - - 0 - 0 - 597 - 347 - - - - Logger Level - - - - - - - - - - Nodes - - - - - - - - - - Refresh - - - - - - - - - - - Loggers - - - - - - - - - - - - - - Levels - - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level b/workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level deleted file mode 100755 index e167c745..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_logger_level.logger_level.LoggerLevel')) diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/setup.py b/workspace/src/rqt_common_plugins/rqt_logger_level/setup.py deleted file mode 100644 index 7e671f79..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_logger_level'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_logger_level'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/__init__.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py deleted file mode 100644 index bb821cf8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py +++ /dev/null @@ -1,70 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin - -from .logger_level_widget import LoggerLevelWidget -from .logger_level_service_caller import LoggerLevelServiceCaller - - -class LoggerLevel(Plugin): - """ - rqt_logger_level plugin's main class. Creates a widget and a - service caller object and displays the widget. - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(LoggerLevel, self).__init__(context) - self.setObjectName('LoggerLevel') - self._service_caller = LoggerLevelServiceCaller() - self._widget = LoggerLevelWidget(self._service_caller) - - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - pass - - def save_settings(self, plugin_settings, instance_settings): - # TODO: implement save functionality for the current logger states - pass - - def restore_settings(self, plugin_settings, instance_settings): - # TODO: implement restore functionality for the current logger states - pass - - #def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure it - # Usually used to open a dialog to offer the user a set of configuration diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py deleted file mode 100644 index 405e33ff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py +++ /dev/null @@ -1,126 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rosnode -import rospy -import rosservice - -from python_qt_binding.QtCore import QObject, qWarning - - -class LoggerLevelServiceCaller(QObject): - """ - Handles service calls for getting lists of nodes and loggers - Also handles sending requests to change logger levels - """ - def __init__(self): - super(LoggerLevelServiceCaller, self).__init__() - - def get_levels(self): - return [self.tr('Debug'), self.tr('Info'), self.tr('Warn'), self.tr('Error'), self.tr('Fatal')] - - def get_loggers(self, node): - if self._refresh_loggers(node): - return self._current_loggers - else: - return [] - - def get_node_names(self): - """ - Gets a list of available services via a ros service call. - :returns: a list of all nodes that provide the set_logger_level service, ''list(str)'' - """ - set_logger_level_nodes = [] - nodes = rosnode.get_node_names() - for name in sorted(nodes): - for service in rosservice.get_service_list(name): - if service == name + '/set_logger_level': - set_logger_level_nodes.append(name) - return set_logger_level_nodes - - def _refresh_loggers(self, node): - """ - Stores a list of loggers available for passed in node - :param node: name of the node to query, ''str'' - """ - self._current_loggers = [] - self._current_levels = {} - servicename = node + '/get_loggers' - try: - service = rosservice.get_service_class_by_name(servicename) - except rosservice.ROSServiceIOException as e: - qWarning('During get_service_class_by_name "%s":\n%s' % (servicename, e)) - return False - request = service._request_class() - proxy = rospy.ServiceProxy(str(servicename), service) - try: - response = proxy(request) - except rospy.ServiceException as e: - qWarning('SetupDialog.get_loggers(): request:\n%s' % (type(request))) - qWarning('SetupDialog.get_loggers() "%s":\n%s' % (servicename, e)) - return False - - if response._slot_types[0] == 'roscpp/Logger[]': - for logger in getattr(response, response.__slots__[0]): - self._current_loggers.append(getattr(logger, 'name')) - self._current_levels[getattr(logger, 'name')] = getattr(logger, 'level') - else: - qWarning(repr(response)) - return False - return True - - def send_logger_change_message(self, node, logger, level): - """ - Sends a logger level change request to 'node'. - :param node: name of the node to chaange, ''str'' - :param logger: name of the logger to change, ''str'' - :param level: name of the level to change, ''str'' - :returns: True if the response is valid, ''bool'' - :returns: False if the request raises an exception or would not change the cached state, ''bool'' - """ - servicename = node + '/set_logger_level' - if self._current_levels[logger].lower() == level.lower(): - return False - - service = rosservice.get_service_class_by_name(servicename) - request = service._request_class() - setattr(request, 'logger', logger) - setattr(request, 'level', level) - proxy = rospy.ServiceProxy(str(servicename), service) - try: - proxy(request) - self._current_levels[logger] = level.upper() - except rospy.ServiceException as e: - qWarning('SetupDialog.level_changed(): request:\n%r' % (request)) - qWarning('SetupDialog.level_changed() "%s":\n%s' % (servicename, e)) - return False - return True diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py deleted file mode 100644 index ac71f16e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py +++ /dev/null @@ -1,126 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import qWarning -from python_qt_binding.QtGui import QWidget - - -class LoggerLevelWidget(QWidget): - """ - Widget for use with LoggerLevelServiceCaller class to alter the ROS logger levels - """ - def __init__(self, caller): - """ - :param caller: service caller instance for sending service calls, ''LoggerLevelServiceCaller'' - """ - super(LoggerLevelWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_logger_level'), 'resource', 'logger_level.ui') - loadUi(ui_file, self) - self.setObjectName('LoggerLevelWidget') - self._caller = caller - - self.node_list.currentRowChanged[int].connect(self.node_changed) - self.logger_list.currentRowChanged[int].connect(self.logger_changed) - self.level_list.currentRowChanged[int].connect(self.level_changed) - self.refresh_button.clicked[bool].connect(self.refresh_nodes) - - self.refresh_nodes() - if self.node_list.count() > 0: - self.node_list.setCurrentRow(0) - - def refresh_nodes(self): - """ - Refreshes the top level node list and repoulates the node_list widget. - As a side effect the level and logger lists are cleared - """ - self.level_list.clear() - self.logger_list.clear() - self.node_list.clear() - for name in self._caller.get_node_names(): - self.node_list.addItem(name) - - def node_changed(self, row): - """ - Handles the rowchanged event for the node_list widget - Populates logger_list with the loggers for the node selected - :param row: the selected row in node_list, ''int'' - """ - if row == -1: - return - if row < 0 or row >= self.node_list.count(): - qWarning('Node row %s out of bounds. Current count: %s' % (row, self.node_list.count())) - return - self.logger_list.clear() - self.level_list.clear() - loggers = self._caller.get_loggers(self.node_list.item(row).text()) - if len(loggers) == 0: - return - for logger in sorted(loggers): - self.logger_list.addItem(logger) - if self.logger_list.count() != 0: - self.logger_list.setCurrentRow(0) - - def logger_changed(self, row): - """ - Handles the rowchanged event for the logger_list widget - Populates level_list with the levels for the logger selected - :param row: the selected row in logger_list, ''int'' - """ - if row == -1: - return - if row < 0 or row >= self.logger_list.count(): - qWarning('Logger row %s out of bounds. Current count: %s' % (row, self.logger_list.count())) - return - if self.level_list.count() == 0: - for level in self._caller.get_levels(): - self.level_list.addItem(level) - for index in range(self.level_list.count()): - if self.level_list.item(index).text().lower() == self._caller._current_levels[self.logger_list.currentItem().text()].lower(): - self.level_list.setCurrentRow(index) - - def level_changed(self, row): - """ - Handles the rowchanged event for the level_list widget - Makes a service call to change the logger level for the indicated node/logger to the selected value - :param row: the selected row in level_list, ''int'' - """ - if row == -1: - return - if row < 0 or row >= self.level_list.count(): - qWarning('Level row %s out of bounds. Current count: %s' % (row, self.level_list.count())) - return - self._caller.send_logger_change_message(self.node_list.currentItem().text(), self.logger_list.currentItem().text(), self.level_list.item(row).text()) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst deleted file mode 100644 index 623d61db..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst +++ /dev/null @@ -1,107 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_msg -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* fix right click view (`#317 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Fix; IndexError: list index out of range (`#26 `_) -* A common module (rqt_msg.MessageWidget) optimized so that other pkgs that provide similar functionality (eg. rqt_srv, rqt_action) can use it - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt deleted file mode 100644 index e40e30e7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_msg) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_msg - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox deleted file mode 100644 index 5fe1b0f3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_msg provides a GUI plugin for introspecting available ROS message types. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_msg/package.xml b/workspace/src/rqt_common_plugins/rqt_msg/package.xml deleted file mode 100644 index c8a745ba..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - rqt_msg - 0.3.13 - A Python GUI plugin for introspecting available ROS message types. - Note that the msgs available through this plugin is the ones that are stored - on your machine, not on the ROS core your rqt instance connects to. - - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_msg - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - python-rospkg - roslib - rosmsg - rospy - rqt_gui - rqt_gui_py - rqt_py_common - rqt_console - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_msg/plugin.xml b/workspace/src/rqt_common_plugins/rqt_msg/plugin.xml deleted file mode 100644 index 5ac26514..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for browsing available ROS message types. - - - - - folder - Plugins related to ROS topics. - - - mail-read - A Python GUI plugin for browsing available ROS message types. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui b/workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui deleted file mode 100644 index fe06ab16..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui +++ /dev/null @@ -1,124 +0,0 @@ - - - MessagesWidget - - - - 0 - 0 - 916 - 561 - - - - Message Type Browser - - - - 0 - - - - - 0 - - - - - 2 - - - - - Message: - - - - - - - - 250 - 0 - - - - Package - - - QComboBox::InsertAlphabetically - - - - - - - / - - - - - - - - 250 - 0 - - - - Message Name - - - - - - - Add Currently Selected Message - - - - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Select a message above and click add to see it in this viewing area. - - - - - - - - - - MessagesTreeView - QTreeView -
rqt_msg.messages_widget
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg b/workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg deleted file mode 100755 index 682a731b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_msg.messages.Messages')) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/setup.py b/workspace/src/rqt_common_plugins/rqt_msg/setup.py deleted file mode 100644 index 9bd3ce73..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_msg'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/__init__.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py deleted file mode 100644 index fb37cf3e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py +++ /dev/null @@ -1,56 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin -from .messages_widget import MessagesWidget - - -class Messages(Plugin): - def __init__(self, context): - super(Messages, self).__init__(context) - self.setObjectName('Messages') - self._widget = MessagesWidget() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # v = instance_settings.value(k) - pass diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py deleted file mode 100644 index a416c3a7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py +++ /dev/null @@ -1,42 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from rqt_py_common.message_tree_model import MessageTreeModel - - -class MessagesTreeModel(MessageTreeModel): - def __init__(self, parent=None): - super(MessagesTreeModel, self).__init__() - self.setHorizontalHeaderLabels([self.tr('Tree'), - self.tr('Type'), - self.tr('Path')]) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py deleted file mode 100644 index f5a99e8c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py +++ /dev/null @@ -1,47 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from rqt_py_common.message_tree_widget import MessageTreeWidget -from .messages_tree_model import MessagesTreeModel - - -class MessagesTreeView(MessageTreeWidget): - def __init__(self, parent=None): - super(MessagesTreeView, self).__init__() - self.setModel(MessagesTreeModel(self)) - - def _recursive_set_editable(self, item, editable=True): - item.setEditable(editable) - for row in range(item.rowCount()): - for col in range(item.columnCount()): - self._recursive_set_editable(item.child(row, col), editable) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py deleted file mode 100644 index ff53fa58..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py +++ /dev/null @@ -1,216 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import (QAction, QIcon, QMenu, QMessageBox, - QTreeView, QWidget) -import roslib -import rosmsg -import rospkg -import rospy - - -from .messages_tree_view import MessagesTreeView -from rqt_py_common import rosaction -from rqt_console.text_browse_dialog import TextBrowseDialog - - -class MessagesWidget(QWidget): - """ - This class is intended to be able to handle msg, srv & action (actionlib). - The name of the class is kept to use message, by following the habit of - rosmsg (a script that can handle both msg & srv). - """ - def __init__(self, mode=rosmsg.MODE_MSG, - pkg_name='rqt_msg', - ui_filename='messages.ui'): - """ - :param ui_filename: This Qt-based .ui file must have elements that are - referred from this class. Otherwise unexpected - errors are likely to happen. Best way to avoid that - situation when you want to give your own .ui file - is to implement all Qt components in - rqt_msg/resource/message.ui file. - """ - - super(MessagesWidget, self).__init__() - self._rospack = rospkg.RosPack() - ui_file = os.path.join(self._rospack.get_path(pkg_name), 'resource', ui_filename) - loadUi(ui_file, self, {'MessagesTreeView': MessagesTreeView}) - self.setObjectName(ui_filename) - self._mode = mode - - self._add_button.setIcon(QIcon.fromTheme('list-add')) - self._add_button.clicked.connect(self._add_message) - self._refresh_packages(mode) - self._refresh_msgs(self._package_combo.itemText(0)) - self._package_combo.currentIndexChanged[str].connect(self._refresh_msgs) - self._messages_tree.mousePressEvent = self._handle_mouse_press - - self._browsers = [] - - def _refresh_packages(self, mode=rosmsg.MODE_MSG): - if (self._mode == rosmsg.MODE_MSG) or self._mode == rosmsg.MODE_SRV: - packages = sorted([pkg_tuple[0] for pkg_tuple in - rosmsg.iterate_packages(self._rospack, self._mode)]) - elif self._mode == rosaction.MODE_ACTION: - packages = sorted([pkg_tuple[0] - for pkg_tuple in rosaction.iterate_packages( - self._rospack, self._mode)]) - self._package_list = packages - rospy.logdebug('pkgs={}'.format(self._package_list)) - self._package_combo.clear() - self._package_combo.addItems(self._package_list) - self._package_combo.setCurrentIndex(0) - - def _refresh_msgs(self, package=None): - if package is None or len(package) == 0: - return - self._msgs = [] - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - msg_list = rosmsg.list_msgs(package) - elif self._mode == rosmsg.MODE_SRV: - msg_list = rosmsg.list_srvs(package) - - rospy.logdebug('_refresh_msgs package={} msg_list={}'.format(package, - msg_list)) - for msg in msg_list: - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - msg_class = roslib.message.get_message_class(msg) - elif self._mode == rosmsg.MODE_SRV: - msg_class = roslib.message.get_service_class(msg) - - rospy.logdebug('_refresh_msgs msg_class={}'.format(msg_class)) - - if msg_class is not None: - self._msgs.append(msg) - - self._msgs = [x.split('/')[1] for x in self._msgs] - - self._msgs_combo.clear() - self._msgs_combo.addItems(self._msgs) - - def _add_message(self): - if self._msgs_combo.count() == 0: - return - msg = (self._package_combo.currentText() + - '/' + self._msgs_combo.currentText()) - - rospy.logdebug('_add_message msg={}'.format(msg)) - - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - msg_class = roslib.message.get_message_class(msg)() - if self._mode == rosmsg.MODE_MSG: - text_tree_root = 'Msg Root' - elif self._mode == rosaction.MODE_ACTION: - text_tree_root = 'Action Root' - self._messages_tree.model().add_message(msg_class, - self.tr(text_tree_root), msg, msg) - - elif self._mode == rosmsg.MODE_SRV: - msg_class = roslib.message.get_service_class(msg)() - self._messages_tree.model().add_message(msg_class._request_class, - self.tr('Service Request'), - msg, msg) - self._messages_tree.model().add_message(msg_class._response_class, - self.tr('Service Response'), - msg, msg) - self._messages_tree._recursive_set_editable( - self._messages_tree.model().invisibleRootItem(), False) - - def _handle_mouse_press(self, event, - old_pressEvent=QTreeView.mousePressEvent): - if (event.buttons() & Qt.RightButton and - event.modifiers() == Qt.NoModifier): - self._rightclick_menu(event) - event.accept() - return old_pressEvent(self._messages_tree, event) - - def _rightclick_menu(self, event): - """ - :type event: QEvent - """ - - # QTreeview.selectedIndexes() returns 0 when no node is selected. - # This can happen when after booting no left-click has been made yet - # (ie. looks like right-click doesn't count). These lines are the - # workaround for that problem. - selected = self._messages_tree.selectedIndexes() - if len(selected) == 0: - return - - menu = QMenu() - text_action = QAction(self.tr('View Text'), menu) - menu.addAction(text_action) - raw_action = QAction(self.tr('View Raw'), menu) - menu.addAction(raw_action) - - action = menu.exec_(event.globalPos()) - - if action == raw_action or action == text_action: - rospy.logdebug('_rightclick_menu selected={}'.format(selected)) - selected_type = selected[1].data() - - if selected_type[-2:] == '[]': - selected_type = selected_type[:-2] - browsetext = None - try: - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - browsetext = rosmsg.get_msg_text(selected_type, - action == raw_action) - elif self._mode == rosmsg.MODE_SRV: - browsetext = rosmsg.get_srv_text(selected_type, - action == raw_action) - - else: - raise - except rosmsg.ROSMsgException, e: - QMessageBox.warning(self, self.tr('Warning'), - self.tr('The selected item component ' + - 'does not have text to view.')) - if browsetext is not None: - self._browsers.append(TextBrowseDialog(browsetext, - self._rospack)) - self._browsers[-1].show() - else: - return - - def cleanup_browsers_on_close(self): - for browser in self._browsers: - browser.close() diff --git a/workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst deleted file mode 100644 index 826e6b5f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst +++ /dev/null @@ -1,125 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_plot -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* Added missing include -* use proper icon names for add/remove -* Contributors: Jochen Sprickerhof, Vincent Rabaud - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* save and restore axes settings (`#234 `_) -* remove warning when backend is not found (`#301 `_) -* fix version clash for matplot backend when PyQt5 is installed (`#299 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- -* fix handling of variable-sized arrays (`#261 `_) - -0.3.8 (2014-07-15) ------------------- -* fix missing installation of Python subpackage - -0.3.7 (2014-07-11) ------------------- -* fix missing import (`#248 `_) -* significant improvements and unification of different plot backends (`#239 `_, `#231 `_) -* make more things plottable including arrays and simple message types (`#246 `_) -* make DataPlot a proxy for its plot widget, redraw after loading new data, add clear_values (`#236 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* subscribe to any known topic, even if currently not available (`#233 `_) - -0.3.5 (2014-05-07) ------------------- -* change minimum padding to enable viewing arbitrarily small values (`#223 `_) -* redraw plot only on new data to reduce cpu load, especially with matplot (`#219 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* add checkbox to toggle automatic scrolling of plot with data -* add simple legend for pyqtgraph backend - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix waiting on unpublished topics (`#110 `_) -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- -* command line arguments enabled - -0.2.7 (2012-12-24) ------------------- -* update mat plot, remove usage of collections and numpy, calculate y range once when adding data instead of on draw (`ros-visualization/rqt#48 `_) -* automatically adjust margins for matplot on resize - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt deleted file mode 100644 index b23f3048..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_plot) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_plot - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox deleted file mode 100644 index 7130ce61..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_plot provides a GUI plugin for visualizing numeric values in a 2D plot using PyQwt5. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_plot/package.xml b/workspace/src/rqt_common_plugins/rqt_plot/package.xml deleted file mode 100644 index 18c0a27b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - rqt_plot - 0.3.13 - rqt_plot provides a GUI plugin visualizing numeric values in a 2D plot using different plotting backends. - Dorian Scholz - Austin Hendrix - - BSD - - http://ros.org/wiki/rqt_plot - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-matplotlib - python-qt-bindings-qwt5 - python-rospkg - qt_gui_py_common - rosgraph - rostopic - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_plot/plugin.xml b/workspace/src/rqt_common_plugins/rqt_plot/plugin.xml deleted file mode 100644 index fb771b83..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A plugin visualizing numeric values in a 2D plot using different plotting backends. - - - - - folder - Plugins related to visualization. - - - utilities-system-monitor - A plugin for visualizing numeric values in a 2D plot using different plotting backends. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui b/workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui deleted file mode 100644 index 8419a72f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui +++ /dev/null @@ -1,157 +0,0 @@ - - - DataPlotWidget - - - - 0 - 0 - 561 - 61 - - - - true - - - Plot - - - - 0 - - - 0 - - - - - 0 - - - 5 - - - - - Topic - - - - - - - / - - - - - - - - 0 - 0 - - - - Qt::DefaultContextMenu - - - add topic to plot - - - - 16 - 16 - - - - - - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - autoscroll - - - true - - - - - - - - 0 - 0 - - - - pause plot - - - - 16 - 16 - - - - true - - - - - - - - 0 - 0 - - - - clear plot - - - - 16 - 16 - - - - - - - - - - 0 - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot b/workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot deleted file mode 100755 index 57e1225a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main -from rqt_plot.plot import Plot - -plugin = 'rqt_plot.plot.Plot' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin, plugin_argument_provider=Plot.add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/setup.py b/workspace/src/rqt_common_plugins/rqt_plot/setup.py deleted file mode 100644 index 4ab6df5b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_plot', 'rqt_plot.data_plot'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_plot'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/__init__.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py deleted file mode 100644 index b2ed810e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py +++ /dev/null @@ -1,526 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2014, Austin Hendrix -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import numpy - -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog -from python_qt_binding import QT_BINDING -from python_qt_binding.QtCore import Qt, qWarning, Signal -from python_qt_binding.QtGui import QColor, QWidget, QHBoxLayout -from rqt_py_common.ini_helper import pack, unpack - -try: - from pyqtgraph_data_plot import PyQtGraphDataPlot -except ImportError: - PyQtGraphDataPlot = None - -try: - from mat_data_plot import MatDataPlot -except ImportError: - MatDataPlot = None - -try: - from qwt_data_plot import QwtDataPlot -except ImportError: - QwtDataPlot = None - -# separate class for DataPlot exceptions, just so that users can differentiate -# errors from the DataPlot widget from exceptions generated by the underlying -# libraries -class DataPlotException(Exception): - pass - -class DataPlot(QWidget): - """A widget for displaying a plot of data - - The DataPlot widget displays a plot, on one of several plotting backends, - depending on which backend(s) are available at runtime. It currently - supports PyQtGraph, MatPlot and QwtPlot backends. - - The DataPlot widget manages the plot backend internally, and can save - and restore the internal state using `save_settings` and `restore_settings` - functions. - - Currently, the user MUST call `restore_settings` before using the widget, - to cause the creation of the enclosed plotting widget. - """ - # plot types in order of priority - plot_types = [ - { - 'title': 'PyQtGraph', - 'widget_class': PyQtGraphDataPlot, - 'description': 'Based on PyQtGraph\n- installer: http://luke.campagnola.me/code/pyqtgraph\n', - 'enabled': PyQtGraphDataPlot is not None, - }, - { - 'title': 'MatPlot', - 'widget_class': MatDataPlot, - 'description': 'Based on MatPlotLib\n- needs most CPU\n- needs matplotlib >= 1.1.0\n- if using PySide: PySide > 1.1.0\n', - 'enabled': MatDataPlot is not None, - }, - { - 'title': 'QwtPlot', - 'widget_class': QwtDataPlot, - 'description': 'Based on QwtPlot\n- does not use timestamps\n- uses least CPU\n- needs Python Qwt bindings\n', - 'enabled': QwtDataPlot is not None, - }, - ] - - # pre-defined colors: - RED=(255, 0, 0) - GREEN=(0, 255, 0) - BLUE=(0, 0, 255) - - SCALE_ALL=1 - SCALE_VISIBLE=2 - SCALE_EXTEND=4 - - _colors = [Qt.blue, Qt.red, Qt.cyan, Qt.magenta, Qt.green, Qt.darkYellow, Qt.black, Qt.darkCyan, Qt.darkRed, Qt.gray] - - limits_changed = Signal() - _redraw = Signal() - _add_curve = Signal(str, str, 'QColor', bool) - - def __init__(self, parent=None): - """Create a new, empty DataPlot - - This will raise a RuntimeError if none of the supported plotting - backends can be found - """ - super(DataPlot, self).__init__(parent) - self._plot_index = 0 - self._color_index = 0 - self._markers_on = False - self._autoscroll = True - - self._autoscale_x = True - self._autoscale_y = DataPlot.SCALE_ALL - - # the backend widget that we're trying to hide/abstract - self._data_plot_widget = None - self._curves = {} - self._vline = None - self._redraw.connect(self._do_redraw) - - self._layout = QHBoxLayout() - self.setLayout(self._layout) - - enabled_plot_types = [pt for pt in self.plot_types if pt['enabled']] - if not enabled_plot_types: - version_info = ' and PySide > 1.1.0' if QT_BINDING == 'pyside' else '' - raise RuntimeError('No usable plot type found. Install at least one of: PyQtGraph, MatPlotLib (at least 1.1.0%s) or Python-Qwt5.' % version_info) - - self._switch_data_plot_widget(self._plot_index) - - self.show() - - def _switch_data_plot_widget(self, plot_index, markers_on=False): - """Internal method for activating a plotting backend by index""" - # check if selected plot type is available - if not self.plot_types[plot_index]['enabled']: - # find other available plot type - for index, plot_type in enumerate(self.plot_types): - if plot_type['enabled']: - plot_index = index - break - - self._plot_index = plot_index - self._markers_on = markers_on - selected_plot = self.plot_types[plot_index] - - if self._data_plot_widget: - x_limits = self.get_xlim() - y_limits = self.get_ylim() - - self._layout.removeWidget(self._data_plot_widget) - self._data_plot_widget.close() - self._data_plot_widget = None - else: - x_limits = [0.0, 10.0] - y_limits = [-0.001, 0.001] - - self._data_plot_widget = selected_plot['widget_class'](self) - self._data_plot_widget.limits_changed.connect(self.limits_changed) - self._add_curve.connect(self._data_plot_widget.add_curve) - self._layout.addWidget(self._data_plot_widget) - - # restore old data - for curve_id in self._curves: - curve = self._curves[curve_id] - self._data_plot_widget.add_curve(curve_id, curve['name'], curve['color'], markers_on) - - if self._vline: - self.vline(*self._vline) - - self.set_xlim(x_limits) - self.set_ylim(y_limits) - self.redraw() - - def _switch_plot_markers(self, markers_on): - self._markers_on = markers_on - self._data_plot_widget._color_index = 0 - - for curve_id in self._curves: - self._data_plot_widget.remove_curve(curve_id) - curve = self._curves[curve_id] - self._data_plot_widget.add_curve(curve_id, curve['name'], curve['color'], markers_on) - - self.redraw() - - # interface out to the managing GUI component: get title, save, restore, - # etc - def getTitle(self): - """get the title of the current plotting backend""" - return self.plot_types[self._plot_index]['title'] - - def save_settings(self, plugin_settings, instance_settings): - """Save the settings associated with this widget - - Currently, this is just the plot type, but may include more useful - data in the future""" - instance_settings.set_value('plot_type', self._plot_index) - xlim = self.get_xlim() - ylim = self.get_ylim() - # convert limits to normal arrays of floats; some backends return numpy - # arrays - xlim = [float(x) for x in xlim] - ylim = [float(y) for y in ylim] - instance_settings.set_value('x_limits', pack(xlim)) - instance_settings.set_value('y_limits', pack(ylim)) - - def restore_settings(self, plugin_settings, instance_settings): - """Restore the settings for this widget - - Currently, this just restores the plot type.""" - self._switch_data_plot_widget(int(instance_settings.value('plot_type', 0))) - xlim = unpack(instance_settings.value('x_limits', [])) - ylim = unpack(instance_settings.value('y_limits', [])) - if xlim: - # convert limits to an array of floats; they're often lists of - # strings - try: - xlim = [float(x) for x in xlim] - self.set_xlim(xlim) - except: - qWarning("Failed to restore X limits") - if ylim: - try: - ylim = [float(y) for y in ylim] - self.set_ylim(ylim) - except: - qWarning("Failed to restore Y limits") - - - def doSettingsDialog(self): - """Present the user with a dialog for choosing the plot backend - - This displays a SimpleSettingsDialog asking the user to choose a - plot type, gets the result, and updates the plot type as necessary - - This method is blocking""" - - marker_settings = [ - { - 'title': 'Show Plot Markers', - 'description': 'Warning: Displaying markers in rqt_plot may cause\n \t high cpu load, especially using PyQtGraph\n', - 'enabled': True, - }] - if self._markers_on: - selected_checkboxes = [0] - else: - selected_checkboxes = [] - - dialog = SimpleSettingsDialog(title='Plot Options') - dialog.add_exclusive_option_group(title='Plot Type', options=self.plot_types, selected_index=self._plot_index) - dialog.add_checkbox_group(title='Plot Markers', options=marker_settings, selected_indexes=selected_checkboxes) - [plot_type, checkboxes] = dialog.get_settings() - if plot_type is not None and plot_type['selected_index'] is not None and self._plot_index != plot_type['selected_index']: - self._switch_data_plot_widget(plot_type['selected_index'], 0 in checkboxes['selected_indexes']) - else: - if checkboxes is not None and self._markers_on != (0 in checkboxes['selected_indexes']): - self._switch_plot_markers(0 in checkboxes['selected_indexes']) - - # interface out to the managing DATA component: load data, update data, - # etc - def autoscroll(self, enabled=True): - """Enable or disable autoscrolling of the plot""" - self._autoscroll = enabled - - def redraw(self): - self._redraw.emit() - - def _do_redraw(self): - """Redraw the underlying plot - - This causes the underlying plot to be redrawn. This is usually used - after adding or updating the plot data""" - if self._data_plot_widget: - self._merged_autoscale() - for curve_id in self._curves: - curve = self._curves[curve_id] - self._data_plot_widget.set_values(curve_id, curve['x'], curve['y']) - self._data_plot_widget.redraw() - - def _get_curve(self, curve_id): - if curve_id in self._curves: - return self._curves[curve_id] - else: - raise DataPlotException("No curve named %s in this DataPlot" % - ( curve_id) ) - - def add_curve(self, curve_id, curve_name, data_x, data_y): - """Add a new, named curve to this plot - - Add a curve named `curve_name` to the plot, with initial data series - `data_x` and `data_y`. - - Future references to this curve should use the provided `curve_id` - - Note that the plot is not redraw automatically; call `redraw()` to make - any changes visible to the user. - """ - curve_color = QColor(self._colors[self._color_index % len(self._colors)]) - self._color_index += 1 - - self._curves[curve_id] = { 'x': numpy.array(data_x), - 'y': numpy.array(data_y), - 'name': curve_name, - 'color': curve_color} - if self._data_plot_widget: - self._add_curve.emit(curve_id, curve_name, curve_color, self._markers_on) - - def remove_curve(self, curve_id): - """Remove the specified curve from this plot""" - # TODO: do on UI thread with signals - if curve_id in self._curves: - del self._curves[curve_id] - if self._data_plot_widget: - self._data_plot_widget.remove_curve(curve_id) - - def update_values(self, curve_id, values_x, values_y): - """Append new data to an existing curve - - `values_x` and `values_y` will be appended to the existing data for - `curve_id` - - Note that the plot is not redraw automatically; call `redraw()` to make - any changes visible to the user. - """ - curve = self._get_curve(curve_id) - curve['x'] = numpy.append(curve['x'], values_x) - curve['y'] = numpy.append(curve['y'], values_y) - # sort resulting data, so we can slice it later - sort_order = curve['x'].argsort() - curve['x'] = curve['x'][sort_order] - curve['y'] = curve['y'][sort_order] - - def clear_values(self, curve_id=None): - """Clear the values for the specified curve, or all curves - - This will erase the data series associaed with `curve_id`, or all - curves if `curve_id` is not present or is None - - Note that the plot is not redraw automatically; call `redraw()` to make - any changes visible to the user. - """ - # clear internal curve representation - if curve_id: - curve = self._get_curve(curve_id) - curve['x'] = numpy.array([]) - curve['y'] = numpy.array([]) - else: - for curve_id in self._curves: - self._curves[curve_id]['x'] = numpy.array([]) - self._curves[curve_id]['y'] = numpy.array([]) - - - def vline(self, x, color=RED): - """Draw a vertical line on the plot - - Draw a line a position X, with the given color - - @param x: position of the vertical line to draw - @param color: optional parameter specifying the color, as tuple of - RGB values from 0 to 255 - """ - self._vline = (x, color) - if self._data_plot_widget: - self._data_plot_widget.vline(x, color) - - # autoscaling methods - def set_autoscale(self, x=None, y=None): - """Change autoscaling of plot axes - - if a parameter is not passed, the autoscaling setting for that axis is - not changed - - @param x: enable or disable autoscaling for X - @param y: set autoscaling mode for Y - """ - if x is not None: - self._autoscale_x = x - if y is not None: - self._autoscale_y = y - - # autoscaling: adjusting the plot bounds fit the data - # autoscrollig: move the plot X window to show the most recent data - # - # what order do we do these adjustments in? - # * assuming the various stages are enabled: - # * autoscale X to bring all data into view - # * else, autoscale X to determine which data we're looking at - # * autoscale Y to fit the data we're viewing - # - # * autoscaling of Y might have several modes: - # * scale Y to fit the entire dataset - # * scale Y to fit the current view - # * increase the Y scale to fit the current view - # - # TODO: incrmenetal autoscaling: only update the autoscaling bounds - # when new data is added - def _merged_autoscale(self): - x_limit = [numpy.inf, -numpy.inf] - if self._autoscale_x: - for curve_id in self._curves: - curve = self._curves[curve_id] - if len(curve['x']) > 0: - x_limit[0] = min(x_limit[0], curve['x'].min()) - x_limit[1] = max(x_limit[1], curve['x'].max()) - elif self._autoscroll: - # get current width of plot - x_limit = self.get_xlim() - x_width = x_limit[1] - x_limit[0] - - # reset the upper x_limit so that we ignore the previous position - x_limit[1] = -numpy.inf - - # get largest X value - for curve_id in self._curves: - curve = self._curves[curve_id] - if len(curve['x']) > 0: - x_limit[1] = max(x_limit[1], curve['x'].max()) - - # set lower limit based on width - x_limit[0] = x_limit[1] - x_width - else: - # don't modify limit, or get it from plot - x_limit = self.get_xlim() - - # set sane limits if our limits are infinite - if numpy.isinf(x_limit[0]): - x_limit[0] = 0.0 - if numpy.isinf(x_limit[1]): - x_limit[1] = 1.0 - - - y_limit = [numpy.inf, -numpy.inf] - if self._autoscale_y: - # if we're extending the y limits, initialize them with the - # current limits - if self._autoscale_y & DataPlot.SCALE_EXTEND: - y_limit = self.get_ylim() - for curve_id in self._curves: - curve = self._curves[curve_id] - start_index = 0 - end_index = len(curve['x']) - - # if we're scaling based on the visible window, find the - # start and end indicies of our window - if self._autoscale_y & DataPlot.SCALE_VISIBLE: - # indexof x_limit[0] in curves['x'] - start_index = curve['x'].searchsorted(x_limit[0]) - # indexof x_limit[1] in curves['x'] - end_index = curve['x'].searchsorted(x_limit[1]) - - # region here is cheap because it is a numpy view and not a - # copy of the underlying data - region = curve['y'][start_index:end_index] - if len(region) > 0: - y_limit[0] = min(y_limit[0], region.min()) - y_limit[1] = max(y_limit[1], region.max()) - - # TODO: compute padding around new min and max values - # ONLY consider data for new values; not - # existing limits, or we'll add padding on top of old - # padding in SCALE_EXTEND mode - # - # pad the min/max - # TODO: invert this padding in get_ylim - #ymin = limits[0] - #ymax = limits[1] - #delta = ymax - ymin if ymax != ymin else 0.1 - #ymin -= .05 * delta - #ymax += .05 * delta - else: - y_limit = self.get_ylim() - - # set sane limits if our limits are infinite - if numpy.isinf(y_limit[0]): - y_limit[0] = 0.0 - if numpy.isinf(y_limit[1]): - y_limit[1] = 1.0 - - self.set_xlim(x_limit) - self.set_ylim(y_limit) - - def get_xlim(self): - """get X limits""" - if self._data_plot_widget: - return self._data_plot_widget.get_xlim() - else: - qWarning("No plot widget; returning default X limits") - return [0.0, 1.0] - - def set_xlim(self, limits): - """set X limits""" - if self._data_plot_widget: - self._data_plot_widget.set_xlim(limits) - else: - qWarning("No plot widget; can't set X limits") - - def get_ylim(self): - """get Y limits""" - if self._data_plot_widget: - return self._data_plot_widget.get_ylim() - else: - qWarning("No plot widget; returning default Y limits") - return [0.0, 10.0] - - def set_ylim(self, limits): - """set Y limits""" - if self._data_plot_widget: - self._data_plot_widget.set_ylim(limits) - else: - qWarning("No plot widget; can't set Y limits") - - # signal on y limit changed? diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py deleted file mode 100644 index 91a26031..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Ye Cheng, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding import QT_BINDING, QT_BINDING_VERSION -if QT_BINDING == 'pyside': - try: - from pkg_resources import parse_version - except: - import re - - def parse_version(s): - return [int(x) for x in re.sub(r'(\.0+)*$', '', s).split('.')] - if parse_version(QT_BINDING_VERSION) <= parse_version('1.1.2'): - raise ImportError('A PySide version newer than 1.1.0 is required.') - -from python_qt_binding.QtCore import Slot, Qt, qWarning, Signal -from python_qt_binding.QtGui import QWidget, QVBoxLayout, QSizePolicy, QColor - -import operator -import matplotlib -if matplotlib.__version__ < '1.1.0': - raise ImportError('A newer matplotlib is required (at least 1.1.0)') - -try: - matplotlib.use('Qt4Agg') - from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas -except ImportError: - # work around bug in dateutil - import sys - import thread - sys.modules['_thread'] = thread - from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas -from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg as NavigationToolbar -from matplotlib.figure import Figure - -import numpy - - -class MatDataPlot(QWidget): - class Canvas(FigureCanvas): - """Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.).""" - def __init__(self, parent=None): - super(MatDataPlot.Canvas, self).__init__(Figure()) - self.axes = self.figure.add_subplot(111) - self.axes.grid(True, color='gray') - self.figure.tight_layout() - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.updateGeometry() - - def resizeEvent(self, event): - super(MatDataPlot.Canvas, self).resizeEvent(event) - self.figure.tight_layout() - - limits_changed = Signal() - - def __init__(self, parent=None): - super(MatDataPlot, self).__init__(parent) - self._canvas = MatDataPlot.Canvas() - self._toolbar = NavigationToolbar(self._canvas, self._canvas) - vbox = QVBoxLayout() - vbox.addWidget(self._toolbar) - vbox.addWidget(self._canvas) - self.setLayout(vbox) - - self._curves = {} - self._current_vline = None - self._canvas.mpl_connect('button_release_event', self._limits_changed) - - def _limits_changed(self, event): - self.limits_changed.emit() - - def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False): - - # adding an empty curve and change the limits, so save and restore them - x_limits = self.get_xlim() - y_limits = self.get_ylim() - if markers_on: - marker_size = 3 - else: - marker_size = 0 - line = self._canvas.axes.plot([], [], 'o-', markersize=marker_size, label=curve_name, linewidth=1, picker=5, color=curve_color.name())[0] - self._curves[curve_id] = line - self._update_legend() - self.set_xlim(x_limits) - self.set_ylim(y_limits) - - def remove_curve(self, curve_id): - curve_id = str(curve_id) - if curve_id in self._curves: - self._curves[curve_id].remove() - del self._curves[curve_id] - self._update_legend() - - def _update_legend(self): - handles, labels = self._canvas.axes.get_legend_handles_labels() - if handles: - hl = sorted(zip(handles, labels), key=operator.itemgetter(1)) - handles, labels = zip(*hl) - self._canvas.axes.legend(handles, labels, loc='upper left') - - def set_values(self, curve, data_x, data_y): - line = self._curves[curve] - line.set_data(data_x, data_y) - - def redraw(self): - self._canvas.axes.grid(True, color='gray') - self._canvas.draw() - - def vline(self, x, color): - # convert color range from (0,255) to (0,1.0) - matcolor=(color[0]/255.0, color[1]/255.0, color[2]/255.0) - if self._current_vline: - self._current_vline.remove() - self._current_vline = self._canvas.axes.axvline(x=x, color=matcolor) - - def set_xlim(self, limits): - self._canvas.axes.set_xbound(lower=limits[0], upper=limits[1]) - - def set_ylim(self, limits): - self._canvas.axes.set_ybound(lower=limits[0], upper=limits[1]) - - def get_xlim(self): - return list(self._canvas.axes.get_xbound()) - - def get_ylim(self): - return list(self._canvas.axes.get_ybound()) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py deleted file mode 100644 index a23b7074..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Slot, Qt, qWarning, Signal -from python_qt_binding.QtGui import QColor, QVBoxLayout, QWidget - -from pyqtgraph import PlotWidget, mkPen, mkBrush -import numpy - - -class PyQtGraphDataPlot(QWidget): - - limits_changed = Signal() - - def __init__(self, parent=None): - super(PyQtGraphDataPlot, self).__init__(parent) - self._plot_widget = PlotWidget() - self._plot_widget.getPlotItem().addLegend() - self._plot_widget.setBackground((255, 255, 255)) - self._plot_widget.setXRange(0, 10, padding=0) - vbox = QVBoxLayout() - vbox.addWidget(self._plot_widget) - self.setLayout(vbox) - self._plot_widget.getPlotItem().sigRangeChanged.connect(self.limits_changed) - - self._curves = {} - self._current_vline = None - - def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False): - pen = mkPen(curve_color, width=1) - symbol = "o" - symbolPen = mkPen(QColor(Qt.black)) - symbolBrush = mkBrush(curve_color) - # this adds the item to the plot and legend - if markers_on: - plot = self._plot_widget.plot(name=curve_name, pen=pen, symbol=symbol, symbolPen=symbolPen, symbolBrush=symbolBrush, symbolSize=4) - else: - plot = self._plot_widget.plot(name=curve_name, pen=pen) - self._curves[curve_id] = plot - - def remove_curve(self, curve_id): - curve_id = str(curve_id) - if curve_id in self._curves: - self._plot_widget.removeItem(self._curves[curve_id]) - del self._curves[curve_id] - self._update_legend() - - def _update_legend(self): - # clear and rebuild legend (there is no remove item method for the legend...) - self._plot_widget.clear() - self._plot_widget.getPlotItem().legend.items = [] - for curve in self._curves.values(): - self._plot_widget.addItem(curve) - if self._current_vline: - self._plot_widget.addItem(self._current_vline) - - def redraw(self): - pass - - def set_values(self, curve_id, data_x, data_y): - curve = self._curves[curve_id] - curve.setData(data_x, data_y) - - def vline(self, x, color): - if self._current_vline: - self._plot_widget.removeItem(self._current_vline) - self._current_vline = self._plot_widget.addLine(x=x, pen=color) - - def set_xlim(self, limits): - # TODO: this doesn't seem to handle fast updates well - self._plot_widget.setXRange(limits[0], limits[1], padding=0) - - def set_ylim(self, limits): - self._plot_widget.setYRange(limits[0], limits[1], padding=0) - - def get_xlim(self): - x_range, _ = self._plot_widget.viewRange() - return x_range - - def get_ylim(self): - _, y_range = self._plot_widget.viewRange() - return y_range diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py deleted file mode 100644 index 8f5070c5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py +++ /dev/null @@ -1,255 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# -*- coding: utf-8 -*- -from __future__ import division -import math -import sys - -from python_qt_binding.QtCore import QEvent, QSize, QPointF, Qt, SIGNAL, Signal, Slot, qWarning -from python_qt_binding.QtGui import QColor, QPen, QBrush, QVector2D -import Qwt - -from numpy import arange, zeros, concatenate - - -# create real QwtDataPlot class -class QwtDataPlot(Qwt.QwtPlot): - mouseCoordinatesChanged = Signal(QPointF) - _num_value_saved = 1000 - _num_values_ploted = 1000 - - limits_changed = Signal() - - def __init__(self, *args): - super(QwtDataPlot, self).__init__(*args) - self.setCanvasBackground(Qt.white) - self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend) - - self._curves = {} - - # TODO: rejigger these internal data structures so that they're easier - # to work with, and easier to use with set_xlim and set_ylim - # this may also entail rejiggering the _time_axis so that it's - # actually time axis data, or just isn't required any more - # new data structure - self._x_limits = [0.0, 10.0] - self._y_limits = [0.0, 10.0] - - # old data structures - self._last_canvas_x = 0 - self._last_canvas_y = 0 - self._pressed_canvas_y = 0 - self._pressed_canvas_x = 0 - self._last_click_coordinates = None - - marker_axis_y = Qwt.QwtPlotMarker() - marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop) - marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine) - marker_axis_y.setYValue(0.0) - marker_axis_y.attach(self) - - self._picker = Qwt.QwtPlotPicker( - Qwt.QwtPlot.xBottom, Qwt.QwtPlot.yLeft, Qwt.QwtPicker.PolygonSelection, - Qwt.QwtPlotPicker.PolygonRubberBand, Qwt.QwtPicker.AlwaysOn, self.canvas() - ) - self._picker.setRubberBandPen(QPen(Qt.blue)) - self._picker.setTrackerPen(QPen(Qt.blue)) - - # Initialize data - self.rescale() - #self.move_canvas(0, 0) - self.canvas().setMouseTracking(True) - self.canvas().installEventFilter(self) - - def eventFilter(self, _, event): - if event.type() == QEvent.MouseButtonRelease: - x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x()) - y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y()) - self._last_click_coordinates = QPointF(x, y) - self.limits_changed.emit() - elif event.type() == QEvent.MouseMove: - x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x()) - y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y()) - coords = QPointF(x, y) - if self._picker.isActive() and self._last_click_coordinates is not None: - toolTip = 'origin x: %.5f, y: %.5f' % (self._last_click_coordinates.x(), self._last_click_coordinates.y()) - delta = coords - self._last_click_coordinates - toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (delta.x(), delta.y(), QVector2D(delta).length()) - else: - toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y' - self.setToolTip(toolTip) - self.mouseCoordinatesChanged.emit(coords) - return False - - def log(self, level, message): - self.emit(SIGNAL('logMessage'), level, message) - - def resizeEvent(self, event): - Qwt.QwtPlot.resizeEvent(self, event) - self.rescale() - - def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False): - curve_id = str(curve_id) - if curve_id in self._curves: - return - curve_object = Qwt.QwtPlotCurve(curve_name) - curve_object.attach(self) - curve_object.setPen(curve_color) - if markers_on: - curve_object.setSymbol(Qwt.QwtSymbol(Qwt.QwtSymbol.Ellipse, QBrush(curve_color), QPen(Qt.black), QSize(4,4))) - self._curves[curve_id] = curve_object - - def remove_curve(self, curve_id): - curve_id = str(curve_id) - if curve_id in self._curves: - self._curves[curve_id].hide() - self._curves[curve_id].attach(None) - del self._curves[curve_id] - - def set_values(self, curve_id, data_x, data_y): - curve = self._curves[curve_id] - curve.setData(data_x, data_y) - - def redraw(self): - self.replot() - - # ---------------------------------------------- - # begin qwtplot internal methods - # ---------------------------------------------- - def rescale(self): - self.setAxisScale(Qwt.QwtPlot.yLeft, - self._y_limits[0], - self._y_limits[1]) - self.setAxisScale(Qwt.QwtPlot.xBottom, - self._x_limits[0], - self._x_limits[1]) - - self._canvas_display_height = self._y_limits[1] - self._y_limits[0] - self._canvas_display_width = self._x_limits[1] - self._x_limits[0] - self.redraw() - - def rescale_axis_x(self, delta__x): - """ - add delta_x to the length of the x axis - """ - x_width = self._x_limits[1] - self._x_limits[0] - x_width += delta__x - self._x_limits[1] = x_width + self._x_limits[0] - self.rescale() - - def scale_axis_y(self, max_value): - """ - set the y axis height to max_value, about the current center - """ - canvas_display_height = max_value - canvas_offset_y = (self._y_limits[1] + self._y_limits[0])/2.0 - y_lower_limit = canvas_offset_y - (canvas_display_height / 2) - y_upper_limit = canvas_offset_y + (canvas_display_height / 2) - self._y_limits = [y_lower_limit, y_upper_limit] - self.rescale() - - def move_canvas(self, delta_x, delta_y): - """ - move the canvas by delta_x and delta_y in SCREEN COORDINATES - """ - canvas_offset_x = delta_x * self._canvas_display_width / self.canvas().width() - canvas_offset_y = delta_y * self._canvas_display_height / self.canvas().height() - self._x_limits = [ l + canvas_offset_x for l in self._x_limits] - self._y_limits = [ l + canvas_offset_y for l in self._y_limits] - self.rescale() - - def mousePressEvent(self, event): - self._last_canvas_x = event.x() - self.canvas().x() - self._last_canvas_y = event.y() - self.canvas().y() - self._pressed_canvas_y = event.y() - self.canvas().y() - - def mouseMoveEvent(self, event): - canvas_x = event.x() - self.canvas().x() - canvas_y = event.y() - self.canvas().y() - if event.buttons() & Qt.MiddleButton: # middle button moves the canvas - delta_x = self._last_canvas_x - canvas_x - delta_y = canvas_y - self._last_canvas_y - self.move_canvas(delta_x, delta_y) - elif event.buttons() & Qt.RightButton: # right button zooms - zoom_factor = max(-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0)) - delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y - self.move_canvas(0, zoom_factor * delta_y * 1.0225) - self.scale_axis_y(max(0.005, self._canvas_display_height - (zoom_factor * self._canvas_display_height))) - self.rescale_axis_x(self._last_canvas_x - canvas_x) - self._last_canvas_x = canvas_x - self._last_canvas_y = canvas_y - - def wheelEvent(self, event): # mouse wheel zooms the y-axis - # y position of pointer in graph coordinates - canvas_y = event.y() - self.canvas().y() - - zoom_factor = max(-0.6, min(0.6, (event.delta() / 120) / 6.0)) - delta_y = (self.canvas().height() / 2.0) - canvas_y - self.move_canvas(0, zoom_factor * delta_y * 1.0225) - - self.scale_axis_y(max(0.0005, self._canvas_display_height - zoom_factor * self._canvas_display_height)) - self.limits_changed.emit() - - - def vline(self, x, color): - qWarning("QwtDataPlot.vline is not implemented yet") - - def set_xlim(self, limits): - self.setAxisScale(Qwt.QwtPlot.xBottom, limits[0], limits[1]) - self._x_limits = limits - - def set_ylim(self, limits): - self.setAxisScale(Qwt.QwtPlot.yLeft, limits[0], limits[1]) - self._y_limits = limits - - def get_xlim(self): - return self._x_limits - - def get_ylim(self): - return self._y_limits - - -if __name__ == '__main__': - from python_qt_binding.QtGui import QApplication - - app = QApplication(sys.argv) - plot = QwtDataPlot() - plot.resize(700, 500) - plot.show() - plot.add_curve(0, '(x/500)^2') - plot.add_curve(1, 'sin(x / 20) * 500') - for i in range(plot._num_value_saved): - plot.update_value(0, (i / 500.0) * (i / 5.0)) - plot.update_value(1, math.sin(i / 20.0) * 500) - - sys.exit(app.exec_()) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py deleted file mode 100644 index 62586d85..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import argparse - -from python_qt_binding import QT_BINDING -from python_qt_binding.QtCore import qDebug -from rqt_gui_py.plugin import Plugin - -from rqt_py_common.ini_helper import pack, unpack - -from .plot_widget import PlotWidget - -from .data_plot import DataPlot - -class Plot(Plugin): - - def __init__(self, context): - super(Plot, self).__init__(context) - self.setObjectName('Plot') - - self._context = context - - self._args = self._parse_args(context.argv()) - self._widget = PlotWidget(initial_topics=self._args.topics, start_paused=self._args.start_paused) - self._data_plot = DataPlot(self._widget) - - # disable autoscaling of X, and set a sane default range - self._data_plot.set_autoscale(x=False) - self._data_plot.set_autoscale(y=DataPlot.SCALE_EXTEND|DataPlot.SCALE_VISIBLE) - self._data_plot.set_xlim([0, 10.0]) - - self._widget.switch_data_plot_widget(self._data_plot) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def _parse_args(self, argv): - parser = argparse.ArgumentParser(prog='rqt_plot', add_help=False) - Plot.add_arguments(parser) - args = parser.parse_args(argv) - - # convert topic arguments into topic names - topic_list = [] - for t in args.topics: - # c_topics is the list of topics to plot - c_topics = [] - # compute combined topic list, t == '/foo/bar1,/baz/bar2' - for sub_t in [x for x in t.split(',') if x]: - # check for shorthand '/foo/field1:field2:field3' - if ':' in sub_t: - base = sub_t[:sub_t.find(':')] - # the first prefix includes a field name, so save then strip it off - c_topics.append(base) - if not '/' in base: - parser.error("%s must contain a topic and field name" % sub_t) - base = base[:base.rfind('/')] - - # compute the rest of the field names - fields = sub_t.split(':')[1:] - c_topics.extend(["%s/%s" % (base, f) for f in fields if f]) - else: - c_topics.append(sub_t) - # #1053: resolve command-line topic names - import rosgraph - c_topics = [rosgraph.names.script_resolve_name('rqt_plot', n) for n in c_topics] - if type(c_topics) == list: - topic_list.extend(c_topics) - else: - topic_list.append(c_topics) - args.topics = topic_list - - return args - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_plot plugin') - group.add_argument('-P', '--pause', action='store_true', dest='start_paused', - help='Start in paused state') - group.add_argument('-e', '--empty', action='store_true', dest='start_empty', - help='Start without restoring previous topics') - group.add_argument('topics', nargs='*', default=[], help='Topics to plot') - - def _update_title(self): - self._widget.setWindowTitle(self._data_plot.getTitle()) - if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - - def save_settings(self, plugin_settings, instance_settings): - self._data_plot.save_settings(plugin_settings, instance_settings) - instance_settings.set_value('autoscroll', self._widget.autoscroll_checkbox.isChecked()) - instance_settings.set_value('topics', pack(self._widget._rosdata.keys())) - - def restore_settings(self, plugin_settings, instance_settings): - autoscroll = instance_settings.value('autoscroll', True) in [True, 'true'] - self._widget.autoscroll_checkbox.setChecked(autoscroll) - self._data_plot.autoscroll(autoscroll) - - self._update_title() - - if len(self._widget._rosdata.keys()) == 0 and not self._args.start_empty: - topics = unpack(instance_settings.value('topics', [])) - if topics: - for topic in topics: - self._widget.add_topic(topic) - - self._data_plot.restore_settings(plugin_settings, instance_settings) - - def trigger_configuration(self): - self._data_plot.doSettingsDialog() - self._update_title() - - def shutdown_plugin(self): - self._widget.clean_up_subscribers() diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py deleted file mode 100644 index a8a30b8e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py +++ /dev/null @@ -1,315 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg -import roslib - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot -from python_qt_binding.QtGui import QAction, QIcon, QMenu, QWidget - -import rospy - -from rqt_py_common.topic_completer import TopicCompleter -from rqt_py_common import topic_helpers - -from . rosplot import ROSData, RosPlotException - -def get_plot_fields(topic_name): - topic_type, real_topic, _ = topic_helpers.get_topic_type(topic_name) - if topic_type is None: - message = "topic %s does not exist" % ( topic_name ) - return [], message - field_name = topic_name[len(real_topic)+1:] - - slot_type, is_array, array_size = roslib.msgs.parse_type(topic_type) - field_class = roslib.message.get_message_class(slot_type) - - fields = [f for f in field_name.split('/') if f] - - for field in fields: - # parse the field name for an array index - try: - field, _, field_index = roslib.msgs.parse_type(field) - except roslib.msgs.MsgSpecException: - message = "invalid field %s in topic %s" % ( field, real_topic ) - return [], message - - if field not in getattr(field_class, '__slots__', []): - message = "no field %s in topic %s" % ( field_name, real_topic ) - return [], message - slot_type = field_class._slot_types[field_class.__slots__.index(field)] - slot_type, slot_is_array, array_size = roslib.msgs.parse_type(slot_type) - is_array = slot_is_array and field_index is None - - field_class = topic_helpers.get_type_class(slot_type) - - if field_class in (int, float): - if is_array: - if array_size is not None: - message = "topic %s is fixed-size numeric array" % ( topic_name ) - return [ "%s[%d]" % (topic_name, i) for i in range(array_size) ], message - else: - message = "topic %s is variable-size numeric array" % ( topic_name ) - return [], message - else: - message = "topic %s is numeric" % ( topic_name ) - return [ topic_name ], message - else: - if not roslib.msgs.is_valid_constant_type(slot_type): - numeric_fields = [] - for i, slot in enumerate(field_class.__slots__): - slot_type = field_class._slot_types[i] - slot_type, is_array, array_size = roslib.msgs.parse_type(slot_type) - slot_class = topic_helpers.get_type_class(slot_type) - if slot_class in (int, float) and not is_array: - numeric_fields.append(slot) - message = "" - if len(numeric_fields) > 0: - message = "%d plottable fields in %s" % ( len(numeric_fields), topic_name ) - else: - message = "No plottable fields in %s" % ( topic_name ) - return [ "%s/%s" % (topic_name, f) for f in numeric_fields ], message - else: - message = "Topic %s is not numeric" % ( topic_name ) - return [], message - -def is_plottable(topic_name): - fields, message = get_plot_fields(topic_name) - return len(fields) > 0, message - -class PlotWidget(QWidget): - _redraw_interval = 40 - - def __init__(self, initial_topics=None, start_paused=False): - super(PlotWidget, self).__init__() - self.setObjectName('PlotWidget') - - self._initial_topics = initial_topics - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_plot'), 'resource', 'plot.ui') - loadUi(ui_file, self) - self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add')) - self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove')) - self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) - self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) - self.data_plot = None - - self.subscribe_topic_button.setEnabled(False) - if start_paused: - self.pause_button.setChecked(True) - - self._topic_completer = TopicCompleter(self.topic_edit) - self._topic_completer.update_topics() - self.topic_edit.setCompleter(self._topic_completer) - - self._start_time = rospy.get_time() - self._rosdata = {} - self._remove_topic_menu = QMenu() - - # init and start update timer for plot - self._update_plot_timer = QTimer(self) - self._update_plot_timer.timeout.connect(self.update_plot) - - def switch_data_plot_widget(self, data_plot): - self.enable_timer(enabled=False) - - self.data_plot_layout.removeWidget(self.data_plot) - if self.data_plot is not None: - self.data_plot.close() - - self.data_plot = data_plot - self.data_plot_layout.addWidget(self.data_plot) - self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) - - # setup drag 'n drop - self.data_plot.dropEvent = self.dropEvent - self.data_plot.dragEnterEvent = self.dragEnterEvent - - if self._initial_topics: - for topic_name in self._initial_topics: - self.add_topic(topic_name) - self._initial_topics = None - else: - for topic_name, rosdata in self._rosdata.items(): - data_x, data_y = rosdata.next() - self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) - - self._subscribed_topics_changed() - - @Slot('QDragEnterEvent*') - def dragEnterEvent(self, event): - # get topic name - if not event.mimeData().hasText(): - if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0: - qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0') - return - item = event.source().selectedItems()[0] - topic_name = item.data(0, Qt.UserRole) - if topic_name == None: - qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)') - return - else: - topic_name = str(event.mimeData().text()) - - # check for plottable field type - plottable, message = is_plottable(topic_name) - if plottable: - event.acceptProposedAction() - else: - qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) - - @Slot('QDropEvent*') - def dropEvent(self, event): - if event.mimeData().hasText(): - topic_name = str(event.mimeData().text()) - else: - droped_item = event.source().selectedItems()[0] - topic_name = str(droped_item.data(0, Qt.UserRole)) - self.add_topic(topic_name) - - @Slot(str) - def on_topic_edit_textChanged(self, topic_name): - # on empty topic name, update topics - if topic_name in ('', '/'): - self._topic_completer.update_topics() - - plottable, message = is_plottable(topic_name) - self.subscribe_topic_button.setEnabled(plottable) - self.subscribe_topic_button.setToolTip(message) - - @Slot() - def on_topic_edit_returnPressed(self): - if self.subscribe_topic_button.isEnabled(): - self.add_topic(str(self.topic_edit.text())) - - @Slot() - def on_subscribe_topic_button_clicked(self): - self.add_topic(str(self.topic_edit.text())) - - @Slot(bool) - def on_pause_button_clicked(self, checked): - self.enable_timer(not checked) - - @Slot(bool) - def on_autoscroll_checkbox_clicked(self, checked): - self.data_plot.autoscroll(checked) - if checked: - self.data_plot.redraw() - - @Slot() - def on_clear_button_clicked(self): - self.clear_plot() - - def update_plot(self): - if self.data_plot is not None: - needs_redraw = False - for topic_name, rosdata in self._rosdata.items(): - try: - data_x, data_y = rosdata.next() - if data_x or data_y: - self.data_plot.update_values(topic_name, data_x, data_y) - needs_redraw = True - except RosPlotException as e: - qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) - if needs_redraw: - self.data_plot.redraw() - - def _subscribed_topics_changed(self): - self._update_remove_topic_menu() - if not self.pause_button.isChecked(): - # if pause button is not pressed, enable timer based on subscribed topics - self.enable_timer(self._rosdata) - self.data_plot.redraw() - - def _update_remove_topic_menu(self): - def make_remove_topic_function(x): - return lambda: self.remove_topic(x) - - self._remove_topic_menu.clear() - for topic_name in sorted(self._rosdata.keys()): - action = QAction(topic_name, self._remove_topic_menu) - action.triggered.connect(make_remove_topic_function(topic_name)) - self._remove_topic_menu.addAction(action) - - if len(self._rosdata) > 1: - all_action = QAction('All', self._remove_topic_menu) - all_action.triggered.connect(self.clean_up_subscribers) - self._remove_topic_menu.addAction(all_action) - - self.remove_topic_button.setMenu(self._remove_topic_menu) - - def add_topic(self, topic_name): - topics_changed = False - for topic_name in get_plot_fields(topic_name)[0]: - if topic_name in self._rosdata: - qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) - continue - self._rosdata[topic_name] = ROSData(topic_name, self._start_time) - if self._rosdata[topic_name].error is not None: - qWarning(str(self._rosdata[topic_name].error)) - del self._rosdata[topic_name] - else: - data_x, data_y = self._rosdata[topic_name].next() - self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) - topics_changed = True - - if topics_changed: - self._subscribed_topics_changed() - - def remove_topic(self, topic_name): - self._rosdata[topic_name].close() - del self._rosdata[topic_name] - self.data_plot.remove_curve(topic_name) - - self._subscribed_topics_changed() - - def clear_plot(self): - for topic_name, _ in self._rosdata.items(): - self.data_plot.clear_values(topic_name) - self.data_plot.redraw() - - def clean_up_subscribers(self): - for topic_name, rosdata in self._rosdata.items(): - rosdata.close() - self.data_plot.remove_curve(topic_name) - self._rosdata = {} - - self._subscribed_topics_changed() - - def enable_timer(self, enabled=True): - if enabled: - self._update_plot_timer.start(self._redraw_interval) - else: - self._update_plot_timer.stop() diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py deleted file mode 100644 index 922583e6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# - -import string -import sys -import threading -import time - -import rosgraph -import roslib.message -import roslib.names -import rospy - - -class RosPlotException(Exception): - pass - - -def _get_topic_type(topic): - """ - subroutine for getting the topic type - (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn) - - :returns: topic type, real topic name, and rest of name referenced - if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str`` - """ - try: - master = rosgraph.Master('/rosplot') - val = master.getTopicTypes() - except: - raise RosPlotException("unable to get list of topics from master") - matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')] - if matches: - t, t_type = matches[0] - if t_type == roslib.names.ANYTYPE: - return None, None, None - if t_type == topic: - return t_type, None - return t_type, t, topic[len(t):] - else: - return None, None, None - - -def get_topic_type(topic): - """ - Get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn) - - :returns: topic type, real topic name, and rest of name referenced - if the \a topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str`` - """ - topic_type, real_topic, rest = _get_topic_type(topic) - if topic_type: - return topic_type, real_topic, rest - else: - return None, None, None - - -class ROSData(object): - """ - Subscriber to ROS topic that buffers incoming data - """ - - def __init__(self, topic, start_time): - self.name = topic - self.start_time = start_time - self.error = None - - self.lock = threading.Lock() - self.buff_x = [] - self.buff_y = [] - - topic_type, real_topic, fields = get_topic_type(topic) - if topic_type is not None: - self.field_evals = generate_field_evals(fields) - data_class = roslib.message.get_message_class(topic_type) - self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb) - else: - self.error = RosPlotException("Can not resolve topic type of %s" % topic) - - def close(self): - self.sub.unregister() - - def _ros_cb(self, msg): - """ - ROS subscriber callback - :param msg: ROS message data - """ - try: - self.lock.acquire() - try: - self.buff_y.append(self._get_data(msg)) - # #944: use message header time if present - if msg.__class__._has_header: - self.buff_x.append(msg.header.stamp.to_sec() - self.start_time) - else: - self.buff_x.append(rospy.get_time() - self.start_time) - #self.axes[index].plot(datax, buff_y) - except AttributeError, e: - self.error = RosPlotException("Invalid topic spec [%s]: %s" % (self.name, str(e))) - finally: - self.lock.release() - - def next(self): - """ - Get the next data in the series - - :returns: [xdata], [ydata] - """ - if self.error: - raise self.error - try: - self.lock.acquire() - buff_x = self.buff_x - buff_y = self.buff_y - self.buff_x = [] - self.buff_y = [] - finally: - self.lock.release() - return buff_x, buff_y - - def _get_data(self, msg): - val = msg - try: - if not self.field_evals: - return float(val) - for f in self.field_evals: - val = f(val) - return float(val) - except IndexError: - self.error = RosPlotException("[%s] index error for: %s" % (self.name, str(val).replace('\n', ', '))) - except TypeError: - self.error = RosPlotException("[%s] value was not numeric: %s" % (self.name, val)) - - -def _array_eval(field_name, slot_num): - """ - :param field_name: name of field to index into, ``str`` - :param slot_num: index of slot to return, ``str`` - :returns: fn(msg_field)->msg_field[slot_num] - """ - def fn(f): - return getattr(f, field_name).__getitem__(slot_num) - return fn - - -def _field_eval(field_name): - """ - :param field_name: name of field to return, ``str`` - :returns: fn(msg_field)->msg_field.field_name - """ - def fn(f): - return getattr(f, field_name) - return fn - - -def generate_field_evals(fields): - try: - evals = [] - fields = [f for f in fields.split('/') if f] - for f in fields: - if '[' in f: - field_name, rest = f.split('[') - slot_num = string.atoi(rest[:rest.find(']')]) - evals.append(_array_eval(field_name, slot_num)) - else: - evals.append(_field_eval(f)) - return evals - except Exception, e: - raise RosPlotException("cannot parse field reference [%s]: %s" % (fields, str(e))) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst deleted file mode 100644 index 0fed08e1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst +++ /dev/null @@ -1,110 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_publisher -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* fixed rqt_publisher plugin to fill message slots for individual fields of primitive arrays -* use proper icon names for add/remove -* Contributors: Robert Haschke, Vincent Rabaud - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix compatibility with Groovy, use queue_size for Python publishers only when available (`#243 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* use queue_size for Python publishers - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- -* fix regression of 0.3.1 (rospack not defined) - -0.3.1 (2013-10-09) ------------------- -* improve performance to fill combo box with message types (`#177 `_) - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt deleted file mode 100644 index d79f80f4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_publisher) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_publisher - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox deleted file mode 100644 index a22054d9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_publisher provides a ROS plugin for publishing arbitrary messages with fixed or computed field values. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/package.xml b/workspace/src/rqt_common_plugins/rqt_publisher/package.xml deleted file mode 100644 index 7301849e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - rqt_publisher - 0.3.13 - rqt_publisher provides a GUI plugin for publishing arbitrary messages with fixed or computed field values. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_publisher - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - qt_gui_py_common - roslib - rosmsg - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml b/workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml deleted file mode 100644 index d17cf61e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin publishing ROS messages. - - - - - folder - Plugins related to ROS topics. - - - media-playback-start - A Python GUI plugin for publishing ROS messages. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui b/workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui deleted file mode 100644 index 091192a3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui +++ /dev/null @@ -1,220 +0,0 @@ - - - PublisherWidget - - - - 0 - 0 - 739 - 406 - - - - Message Publisher - - - - - - - - Refresh topics and types - - - - - - - - - - - 0 - 0 - - - - Topic - - - - - - - - 1 - 0 - - - - - 100 - 0 - - - - Topic name to publish on - - - true - - - QComboBox::AdjustToMinimumContentsLength - - - - - - - Type - - - - - - - - 2 - 0 - - - - - 100 - 0 - - - - Message type to publish - - - true - - - QComboBox::AdjustToMinimumContentsLength - - - - - - - Freq. - - - - - - - - 60 - 0 - - - - Frequency for periodic publishers in Hz (set to 0 for manual publishing) - - - true - - - - 1 - - - - - - - - Hz - - - - - - - Add new publisher - - - - - - - - - - Remove selected publisher - - - - - - Del - - - - - - - Qt::Horizontal - - - - 20 - 20 - - - - - - - - Clear all publishers - - - - - - - - - - - - Qt::CustomContextMenu - - - Right click on item or selection for more options. - - - true - - - QAbstractItemView::DragOnly - - - QAbstractItemView::ExtendedSelection - - - true - - - - - - - - ExtendedComboBox - QComboBox -
rqt_py_common.extended_combo_box
- - - PublisherTreeWidget - QTreeView -
rqt_publisher.publisher_tree_widget
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher b/workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher deleted file mode 100755 index 6a27309c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_publisher.publisher.Publisher')) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/setup.py b/workspace/src/rqt_common_plugins/rqt_publisher/setup.py deleted file mode 100644 index 61d0fe22..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_publisher'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/__init__.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py deleted file mode 100644 index 60a02c33..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py +++ /dev/null @@ -1,342 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import math -import random -import time - -from python_qt_binding.QtCore import Slot, QSignalMapper, QTimer, qWarning - -import roslib -import rospy -import genpy -from rqt_gui_py.plugin import Plugin -from .publisher_widget import PublisherWidget -from rqt_py_common.topic_helpers import get_field_type - - -class Publisher(Plugin): - - def __init__(self, context): - super(Publisher, self).__init__(context) - self.setObjectName('Publisher') - - # create widget - self._widget = PublisherWidget() - self._widget.add_publisher.connect(self.add_publisher) - self._widget.change_publisher.connect(self.change_publisher) - self._widget.publish_once.connect(self.publish_once) - self._widget.remove_publisher.connect(self.remove_publisher) - self._widget.clean_up_publishers.connect(self.clean_up_publishers) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - # create context for the expression eval statement - self._eval_locals = {'i': 0} - for module in (math, random, time): - self._eval_locals.update(module.__dict__) - self._eval_locals['genpy'] = genpy - del self._eval_locals['__name__'] - del self._eval_locals['__doc__'] - - self._publishers = {} - self._id_counter = 0 - - self._timeout_mapper = QSignalMapper(self) - self._timeout_mapper.mapped[int].connect(self.publish_once) - - # add our self to the main window - context.add_widget(self._widget) - - @Slot(str, str, float, bool) - def add_publisher(self, topic_name, type_name, rate, enabled): - publisher_info = { - 'topic_name': str(topic_name), - 'type_name': str(type_name), - 'rate': float(rate), - 'enabled': bool(enabled), - } - self._add_publisher(publisher_info) - - def _add_publisher(self, publisher_info): - publisher_info['publisher_id'] = self._id_counter - self._id_counter += 1 - publisher_info['counter'] = 0 - publisher_info['enabled'] = publisher_info.get('enabled', False) - publisher_info['expressions'] = publisher_info.get('expressions', {}) - - publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name']) - if publisher_info['message_instance'] is None: - return - - # create publisher and timer - try: - publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']), queue_size=100) - except TypeError: - publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance'])) - publisher_info['timer'] = QTimer(self) - - # add publisher info to _publishers dict and create signal mapping - self._publishers[publisher_info['publisher_id']] = publisher_info - self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id']) - publisher_info['timer'].timeout.connect(self._timeout_mapper.map) - if publisher_info['enabled'] and publisher_info['rate'] > 0: - publisher_info['timer'].start(int(1000.0 / publisher_info['rate'])) - - self._widget.publisher_tree_widget.model().add_publisher(publisher_info) - - @Slot(int, str, str, str, object) - def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback): - handler = getattr(self, '_change_publisher_%s' % column_name, None) - if handler is not None: - new_text = handler(self._publishers[publisher_id], topic_name, new_value) - if new_text is not None: - setter_callback(new_text) - - def _change_publisher_topic(self, publisher_info, topic_name, new_value): - publisher_info['enabled'] = (new_value and new_value.lower() in ['1', 'true', 'yes']) - #qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled'])) - if publisher_info['enabled'] and publisher_info['rate'] > 0: - publisher_info['timer'].start(int(1000.0 / publisher_info['rate'])) - else: - publisher_info['timer'].stop() - return None - - def _change_publisher_type(self, publisher_info, topic_name, new_value): - type_name = new_value - # create new slot - slot_value = self._create_message_instance(type_name) - - # find parent slot - slot_path = topic_name[len(publisher_info['topic_name']):].strip('/').split('/') - parent_slot = eval('.'.join(["publisher_info['message_instance']"] + slot_path[:-1])) - - # find old slot - slot_name = slot_path[-1] - slot_index = parent_slot.__slots__.index(slot_name) - - # restore type if user value was invalid - if slot_value is None: - qWarning('Publisher._change_publisher_type(): could not find type: %s' % (type_name)) - return parent_slot._slot_types[slot_index] - - else: - # replace old slot - parent_slot._slot_types[slot_index] = type_name - setattr(parent_slot, slot_name, slot_value) - - self._widget.publisher_tree_widget.model().update_publisher(publisher_info) - - def _change_publisher_rate(self, publisher_info, topic_name, new_value): - try: - rate = float(new_value) - except Exception: - qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' % (new_value)) - else: - publisher_info['rate'] = rate - #qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate'])) - publisher_info['timer'].stop() - if publisher_info['enabled'] and publisher_info['rate'] > 0: - publisher_info['timer'].start(int(1000.0 / publisher_info['rate'])) - # make sure the column value reflects the actual rate - return '%.2f' % publisher_info['rate'] - - def _change_publisher_expression(self, publisher_info, topic_name, new_value): - expression = str(new_value) - if len(expression) == 0: - if topic_name in publisher_info['expressions']: - del publisher_info['expressions'][topic_name] - #qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name)) - else: - slot_type, is_array = get_field_type(topic_name) - if is_array: - slot_type = list - # strip possible trailing error message from expression - error_prefix = '# error' - error_prefix_pos = expression.find(error_prefix) - if error_prefix_pos >= 0: - expression = expression[:error_prefix_pos] - success, _ = self._evaluate_expression(expression, slot_type) - if success: - old_expression = publisher_info['expressions'].get(topic_name, None) - publisher_info['expressions'][topic_name] = expression - #print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value) - self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter']) - try: - publisher_info['message_instance']._check_types() - except Exception, e: - error_str = str(e) - print 'serialization error:', error_str - if old_expression is not None: - publisher_info['expressions'][topic_name] = old_expression - else: - del publisher_info['expressions'][topic_name] - return '%s %s: %s' % (expression, error_prefix, error_str) - return expression - else: - return '%s %s evaluating as "%s"' % (expression, error_prefix, slot_type.__name__) - - def _extract_array_info(self, type_str): - array_size = None - if '[' in type_str and type_str[-1] == ']': - type_str, array_size_str = type_str.split('[', 1) - array_size_str = array_size_str[:-1] - if len(array_size_str) > 0: - array_size = int(array_size_str) - else: - array_size = 0 - - return type_str, array_size - - def _create_message_instance(self, type_str): - base_type_str, array_size = self._extract_array_info(type_str) - - base_message_type = roslib.message.get_message_class(base_type_str) - if base_message_type is None: - print 'Could not create message of type "%s".' % base_type_str - return None - - if array_size is not None: - message = [] - for _ in range(array_size): - message.append(base_message_type()) - else: - message = base_message_type() - return message - - def _evaluate_expression(self, expression, slot_type): - successful_eval = True - - try: - # try to evaluate expression - value = eval(expression, {}, self._eval_locals) - except Exception: - successful_eval = False - - if slot_type is str: - if successful_eval: - value = str(value) - else: - # for string slots just convert the expression to str, if it did not evaluate successfully - value = str(expression) - successful_eval = True - - elif successful_eval: - type_set = set((slot_type, type(value))) - # check if value's type and slot_type belong to the same type group, i.e. array types, numeric types - # and if they do, make sure values's type is converted to the exact slot_type - if type_set <= set((list, tuple)) or type_set <= set((int, float)): - # convert to the right type - value = slot_type(value) - - if successful_eval and isinstance(value, slot_type): - return True, value - else: - qWarning('Publisher._evaluate_expression(): failed to evaluate expression: "%s" as Python type "%s"' % (expression, slot_type.__name__)) - - return False, None - - def _fill_message_slots(self, message, topic_name, expressions, counter): - if topic_name in expressions and len(expressions[topic_name]) > 0: - - # get type - if hasattr(message, '_type'): - message_type = message._type - else: - message_type = type(message) - - self._eval_locals['i'] = counter - success, value = self._evaluate_expression(expressions[topic_name], message_type) - if not success: - value = message_type() - return value - - # if no expression exists for this topic_name, continue with it's child slots - elif hasattr(message, '__slots__'): - for slot_name in message.__slots__: - value = self._fill_message_slots(getattr(message, slot_name), topic_name + '/' + slot_name, expressions, counter) - if value is not None: - setattr(message, slot_name, value) - - elif type(message) in (list, tuple) and (len(message) > 0): - for index, slot in enumerate(message): - value = self._fill_message_slots(slot, topic_name + '[%d]' % index, expressions, counter) - # this deals with primitive-type arrays - if not hasattr(message[0], '__slots__') and value is not None: - message[index] = value - - return None - - @Slot(int) - def publish_once(self, publisher_id): - publisher_info = self._publishers.get(publisher_id, None) - if publisher_info is not None: - publisher_info['counter'] += 1 - self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter']) - publisher_info['publisher'].publish(publisher_info['message_instance']) - - @Slot(int) - def remove_publisher(self, publisher_id): - publisher_info = self._publishers.get(publisher_id, None) - if publisher_info is not None: - publisher_info['timer'].stop() - publisher_info['publisher'].unregister() - del self._publishers[publisher_id] - - def save_settings(self, plugin_settings, instance_settings): - publisher_copies = [] - for publisher in self._publishers.values(): - publisher_copy = {} - publisher_copy.update(publisher) - publisher_copy['enabled'] = False - del publisher_copy['timer'] - del publisher_copy['message_instance'] - del publisher_copy['publisher'] - publisher_copies.append(publisher_copy) - instance_settings.set_value('publishers', repr(publisher_copies)) - - def restore_settings(self, plugin_settings, instance_settings): - publishers = eval(instance_settings.value('publishers', '[]')) - for publisher in publishers: - self._add_publisher(publisher) - - def clean_up_publishers(self): - self._widget.publisher_tree_widget.model().clear() - for publisher_info in self._publishers.values(): - publisher_info['timer'].stop() - publisher_info['publisher'].unregister() - self._publishers = {} - - def shutdown_plugin(self): - self._widget.shutdown_plugin() - self.clean_up_publishers() diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py deleted file mode 100644 index 86cb18cc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -import threading - -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QStandardItem - -from rqt_py_common.message_tree_model import MessageTreeModel -from rqt_py_common.data_items import ReadonlyItem, CheckableItem - - -class PublisherTreeModel(MessageTreeModel): - _column_names = ['topic', 'type', 'rate', 'expression'] - item_value_changed = Signal(int, str, str, str, object) - - def __init__(self, parent=None): - super(PublisherTreeModel, self).__init__(parent) - self._column_index = {} - for column_name in self._column_names: - self._column_index[column_name] = len(self._column_index) - self.clear() - - self._item_change_lock = threading.Lock() - self.itemChanged.connect(self.handle_item_changed) - - def clear(self): - super(PublisherTreeModel, self).clear() - self.setHorizontalHeaderLabels(self._column_names) - - def get_publisher_ids(self, index_list): - return [item._user_data['publisher_id'] for item in self._get_toplevel_items(index_list)] - - def remove_items_with_parents(self, index_list): - for item in self._get_toplevel_items(index_list): - self.removeRow(item.row()) - - def handle_item_changed(self, item): - if not self._item_change_lock.acquire(False): - #qDebug('PublisherTreeModel.handle_item_changed(): could not acquire lock') - return - # lock has been acquired - topic_name = item._path - column_name = self._column_names[item.column()] - if item.isCheckable(): - new_value = str(item.checkState() == Qt.Checked) - else: - new_value = item.text().strip() - #print 'PublisherTreeModel.handle_item_changed(): %s, %s, %s' % (topic_name, column_name, new_value) - - self.item_value_changed.emit(item._user_data['publisher_id'], topic_name, column_name, new_value, item.setText) - - # release lock - self._item_change_lock.release() - - def remove_publisher(self, publisher_id): - for top_level_row_number in range(self.rowCount()): - item = self.item(top_level_row_number) - if item is not None and item._user_data['publisher_id'] == publisher_id: - self.removeRow(top_level_row_number) - return top_level_row_number - return None - - def update_publisher(self, publisher_info): - top_level_row_number = self.remove_publisher(publisher_info['publisher_id']) - self.add_publisher(publisher_info, top_level_row_number) - - def add_publisher(self, publisher_info, top_level_row_number=None): - # recursively create widget items for the message's slots - parent = self - slot = publisher_info['message_instance'] - slot_name = publisher_info['topic_name'] - slot_type_name = publisher_info['message_instance']._type - slot_path = publisher_info['topic_name'] - user_data = {'publisher_id': publisher_info['publisher_id']} - kwargs = { - 'user_data': user_data, - 'top_level_row_number': top_level_row_number, - 'expressions': publisher_info['expressions'], - } - top_level_row = self._recursive_create_items(parent, slot, slot_name, slot_type_name, slot_path, **kwargs) - - # fill tree widget columns of top level item - if publisher_info['enabled']: - top_level_row[self._column_index['topic']].setCheckState(Qt.Checked) - top_level_row[self._column_index['rate']].setText(str(publisher_info['rate'])) - - def _get_data_items_for_path(self, slot_name, slot_type_name, slot_path, **kwargs): - if slot_name.startswith('/'): - return (CheckableItem(slot_name), ReadonlyItem(slot_type_name), QStandardItem(''), ReadonlyItem('')) - expression_item = QStandardItem('') - expression_item.setToolTip('enter valid Python expression here, using "i" as counter and functions from math, random and time modules') - return (ReadonlyItem(slot_name), QStandardItem(slot_type_name), ReadonlyItem(''), expression_item) - - def _recursive_create_items(self, parent, slot, slot_name, slot_type_name, slot_path, expressions={}, **kwargs): - row, is_leaf_node = super(PublisherTreeModel, self)._recursive_create_items(parent, slot, slot_name, slot_type_name, slot_path, expressions=expressions, **kwargs) - if is_leaf_node: - expression_text = expressions.get(slot_path, repr(slot)) - row[self._column_index['expression']].setText(expression_text) - return row diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py deleted file mode 100644 index 9bb3c479..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Signal, Slot -from python_qt_binding.QtGui import QAction, QIcon - -from .publisher_tree_model import PublisherTreeModel -from rqt_py_common.message_tree_widget import MessageTreeWidget -from rqt_py_common.item_delegates import SpinBoxDelegate - - -class PublisherTreeWidget(MessageTreeWidget): - remove_publisher = Signal(int) - publish_once = Signal(int) - - def __init__(self, parent=None): - super(PublisherTreeWidget, self).__init__(parent) - self.setModel(PublisherTreeModel(self)) - self._action_remove_publisher = QAction(QIcon.fromTheme('list-remove'), 'Remove Selected', self) - self._action_remove_publisher.triggered[bool].connect(self._handle_action_remove_publisher) - self._action_publish_once = QAction(QIcon.fromTheme('media-playback-start'), 'Publish Selected Once', self) - self._action_publish_once.triggered[bool].connect(self._handle_action_publish_once) - self.setItemDelegateForColumn(self.model()._column_index['rate'], SpinBoxDelegate(min_value=0, max_value=1000000, decimals=2)) - - @Slot() - def remove_selected_publishers(self): - publisher_ids = self.model().get_publisher_ids(self.selectedIndexes()) - for publisher_id in publisher_ids: - self.remove_publisher.emit(publisher_id) - self.model().remove_items_with_parents(self.selectedIndexes()) - - def _context_menu_add_actions(self, menu, pos): - if self.selectionModel().hasSelection(): - menu.addAction(self._action_remove_publisher) - menu.addAction(self._action_publish_once) - # let super class add actions - super(PublisherTreeWidget, self)._context_menu_add_actions(menu, pos) - - def _handle_action_remove_publisher(self, checked): - self.remove_selected_publishers() - - def _handle_action_publish_once(self, checked): - for publisher_id in self.model().get_publisher_ids(self.selectedIndexes()): - self.publish_once.emit(publisher_id) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py deleted file mode 100644 index 686d237e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Signal, Slot -from python_qt_binding.QtGui import QIcon, QWidget - -import roslib -import rosmsg -import rospkg -import rospy - -from qt_gui_py_common.worker_thread import WorkerThread -from rqt_py_common.extended_combo_box import ExtendedComboBox -from .publisher_tree_widget import PublisherTreeWidget - - -# main class inherits from the ui window class -class PublisherWidget(QWidget): - add_publisher = Signal(str, str, float, bool) - change_publisher = Signal(int, str, str, str, object) - publish_once = Signal(int) - remove_publisher = Signal(int) - clean_up_publishers = Signal() - - def __init__(self, parent=None): - super(PublisherWidget, self).__init__(parent) - self._topic_dict = {} - self._update_thread = WorkerThread(self._update_thread_run, self._update_finished) - - self._rospack = rospkg.RosPack() - ui_file = os.path.join(self._rospack.get_path('rqt_publisher'), 'resource', 'Publisher.ui') - loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox, 'PublisherTreeWidget': PublisherTreeWidget}) - self.refresh_button.setIcon(QIcon.fromTheme('view-refresh')) - self.refresh_button.clicked.connect(self.refresh_combo_boxes) - self.add_publisher_button.setIcon(QIcon.fromTheme('list-add')) - self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove')) - self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) - - self.refresh_combo_boxes() - - self.publisher_tree_widget.model().item_value_changed.connect(self.change_publisher) - self.publisher_tree_widget.remove_publisher.connect(self.remove_publisher) - self.publisher_tree_widget.publish_once.connect(self.publish_once) - self.remove_publisher_button.clicked.connect(self.publisher_tree_widget.remove_selected_publishers) - self.clear_button.clicked.connect(self.clean_up_publishers) - - def shutdown_plugin(self): - self._update_thread.kill() - - @Slot() - def refresh_combo_boxes(self): - self._update_thread.kill() - self.type_combo_box.setEnabled(False) - self.topic_combo_box.setEnabled(False) - self.type_combo_box.setEditText('updating...') - self.topic_combo_box.setEditText('updating...') - self._update_thread.start() - - # this runs in a non-gui thread, so don't access widgets here directly - def _update_thread_run(self): - # update type_combo_box - message_type_names = [] - try: - # this only works on fuerte and up - packages = sorted([pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(self._rospack, rosmsg.MODE_MSG)]) - except: - # this works up to electric - packages = sorted(rosmsg.list_packages()) - for package in packages: - for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack): - message_class = roslib.message.get_message_class(base_type_str) - if message_class is not None: - message_type_names.append(base_type_str) - - self.type_combo_box.setItems.emit(sorted(message_type_names)) - - # update topic_combo_box - _, _, topic_types = rospy.get_master().getTopicTypes() - self._topic_dict = dict(topic_types) - self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys())) - - @Slot() - def _update_finished(self): - self.type_combo_box.setEnabled(True) - self.topic_combo_box.setEnabled(True) - - @Slot(str) - def on_topic_combo_box_currentIndexChanged(self, topic_name): - if topic_name in self._topic_dict: - self.type_combo_box.setEditText(self._topic_dict[topic_name]) - - @Slot() - def on_add_publisher_button_clicked(self): - topic_name = str(self.topic_combo_box.currentText()) - type_name = str(self.type_combo_box.currentText()) - rate = float(self.frequency_combo_box.currentText()) - enabled = False - self.add_publisher.emit(topic_name, type_name, rate, enabled) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst deleted file mode 100644 index 97d29b38..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst +++ /dev/null @@ -1,108 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_py_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* added unittest for MessageTreeModel -* fixed message_tree_model to list individual elements of simple-type array elements -* fixed topic_completer to handle array subscriptions -* Contributors: Robert Haschke - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* improve topic helpers to make more things plottable (`#246 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* fix ui loading of plugin constainer widget - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Add common system messaging pane widget - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt deleted file mode 100644 index f40947d9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_py_common) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - find_package(genmsg REQUIRED) - find_package(std_msgs REQUIRED) - add_message_files( - DIRECTORY test/msg - FILES ArrayVal.msg Val.msg - NOINSTALL - ) - generate_messages(DEPENDENCIES std_msgs) -endif() - -catkin_package() - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test) -endif() \ No newline at end of file diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/package.xml b/workspace/src/rqt_common_plugins/rqt_py_common/package.xml deleted file mode 100644 index 6a1deb7b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/package.xml +++ /dev/null @@ -1,42 +0,0 @@ - - rqt_py_common - 0.3.13 - - rqt_py_common provides common functionality for rqt plugins written in Python. - Despite no plugin is provided, this package is part of the rqt_common_plugins - repository to keep refactoring generic functionality from these common plugins - into this package as easy as possible. - - Functionality included in this package should cover generic ROS concepts and - should not introduce any special dependencies beside "ros_base". - - Dorian Scholz - BSD - - http://ros.org/wiki/rqt_py_common - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - Dorian Scholz - Isaac Saito - - catkin - - genmsg - std_msgs - - genpy - qt_gui - roslib - rospy - rostopic - - - actionlib - rosbag - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui b/workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui deleted file mode 100644 index 555f7549..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui +++ /dev/null @@ -1,78 +0,0 @@ - - - _plugincontainer_top_widget - - - - 0 - 0 - 658 - 523 - - - - - 200 - 250 - - - - PluginContainer Widget - - - - 0 - - - 0 - - - - - Qt::Vertical - - - 9 - - - - - QLayout::SetMaximumSize - - - - - - 200 - 75 - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(System message might be shown here when necessary)</p></body></html> - - - - - - - - 200 - 30 - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/setup.py b/workspace/src/rqt_common_plugins/rqt_py_common/setup.py deleted file mode 100644 index 204762c5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_py_common'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/__init__.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py deleted file mode 100644 index abe7ef31..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QStandardItem - - -class ReadonlyItem(QStandardItem): - def __init__(self, *args): - super(ReadonlyItem, self).__init__(*args) - self.setEditable(False) - - -class CheckableItem(ReadonlyItem): - def __init__(self, *args): - super(CheckableItem, self).__init__(*args) - self.setCheckable(True) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py deleted file mode 100644 index 48de4d9b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Qt, Signal, Slot -from python_qt_binding.QtGui import QComboBox, QCompleter, QSortFilterProxyModel - - -class ExtendedComboBox(QComboBox): - setItems = Signal(list) - - def __init__(self, parent=None): - super(ExtendedComboBox, self).__init__(parent) - - self.setFocusPolicy(Qt.StrongFocus) - self.setEditable(True) - - # add a filter model to filter matching items - self.filter_model = QSortFilterProxyModel(self) - self.filter_model.setFilterCaseSensitivity(Qt.CaseInsensitive) - self.filter_model.setSourceModel(self.model()) - - # add a completer, which uses the filter model - self.completer = QCompleter(self.filter_model, self) - # always show all (filtered) completions - self.completer.setCompletionMode(QCompleter.UnfilteredPopupCompletion) - self.setCompleter(self.completer) - - # connect signals - self.lineEdit().textEdited[unicode].connect(self.filter_model.setFilterFixedString) - self.completer.activated.connect(self.on_completer_activated) - self.setItems.connect(self.onSetItems) - - # on selection of an item from the completer, select the corresponding item from combobox - def on_completer_activated(self, text): - if text: - index = self.findText(text) - self.setCurrentIndex(index) - - # on model change, update the models of the filter and completer as well - def setModel(self, model): - super(ExtendedComboBox, self).setModel(model) - self.filter_model.setSourceModel(model) - self.completer.setModel(self.filter_model) - - # on model column change, update the model column of the filter and completer as well - def setModelColumn(self, column): - self.completer.setCompletionColumn(column) - self.filter_model.setFilterKeyColumn(column) - super(ExtendedComboBox, self).setModelColumn(column) - - @Slot(list) - def onSetItems(self, items): - self.clear() - self.addItems(items) - - -if __name__ == "__main__": - import sys - from python_qt_binding.QtGui import QApplication - - app = QApplication(sys.argv) - - string_list = ['hola muchachos', 'adios amigos', 'hello world', 'good bye'] - - combo = ExtendedComboBox() - - # either fill the standard model of the combobox - combo.addItems(string_list) - - # or use another model - #combo.setModel(QStringListModel(string_list)) - - combo.resize(300, 40) - combo.show() - - sys.exit(app.exec_()) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py deleted file mode 100644 index 3cf40f1e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py +++ /dev/null @@ -1,66 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -def pack(data): - """ - Packs 'data' into a form that can be easily and readably written to an ini file - :param data: A list of strings or a list of items with a 'text' method to be flattened into a string ''list'' - :return: A string suitable for output to ini files ''str'' - """ - if len(data) == 0: - return '' - - def _get_str(item): - try: - return item.text() - except: - return item - - data = [_get_str(value) for value in data] - if len(data) == 1: - return data[0] - return data - - -def unpack(data): - """ - Unpacks the values read from an ini file - :param data: An entry taken from an ini file ''list or string'' - :return: A list of strings ''list'' - """ - if data is None or data == '': - data = [] - elif isinstance(data, basestring): - data = [data] - return data - diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py deleted file mode 100644 index 478933e1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QModelIndex -from python_qt_binding.QtGui import QDoubleSpinBox, QItemDelegate - - -class SpinBoxDelegate(QItemDelegate): - def __init__(self, min_value=0, max_value=100, decimals=2, *args): - self._min = min_value - self._max = max_value - self._decimals = decimals - super(SpinBoxDelegate, self).__init__(*args) - - def createEditor(self, parent, option, index): - editor = QDoubleSpinBox(parent) - editor.setDecimals(self._decimals) - editor.setMaximum(self._min) - editor.setMaximum(self._max) - return editor - - -class DelegateUtil(object): - """ - Find out the hierarchy level of the selected item. - see: http://stackoverflow.com/a/4208240/577001 - - :type model_index: QModelIndex - :rtype: int - - :author: Isaac Saito - """ - @staticmethod - def _get_hierarchy_level(model_index): - hierarchyLevel = 1 - seek_root = model_index - while(seek_root.parent() != QModelIndex()): - seek_root = seek_root.parent() - hierarchyLevel += 1 - return hierarchyLevel diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py deleted file mode 100644 index 6a38c5e7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py +++ /dev/null @@ -1,103 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QWidgetItem -import roslib -import rospy - - -class LayoutUtil(object): - - @staticmethod - def alternate_color(list_widgets, colors_alter=[Qt.white, Qt.gray]): - """ - Alternate the background color of the widgets that are ordered - linearly, by the given list of colors. - - Originally intended for the elements of QHBoxLayout & QVBoxLayout. - - @type list_widgets: QtGui.QWidget[] - @type colors_alter: QtCore.Qt.GlobalColor[] - @param colors_alter: 1st element is used as initial/default color. - @rtype: void - - @author: Isaac Saito - """ - - colors_num = len(colors_alter) - i_widget = 0 - for w in list_widgets: - w.setAutoFillBackground(True) - p = w.palette() - - divisor = (i_widget + colors_num) % colors_num - i_widget += 1 - - rospy.logdebug('LayoutUtil divisor={} i_widget={} colors_num={}'.format( - divisor, - i_widget, - colors_num)) - - p.setColor(w.backgroundRole(), colors_alter[divisor]) - w.setPalette(p) - - @staticmethod - def clear_layout(layout): - """ - Clear all items in the given layout. Currently, only the instances of - QWidgetItem get cleared (ie. QSpaceItem is ignored). - - Originally taken from http://stackoverflow.com/a/9375273/577001 - - :type layout: QLayout - """ - for i in reversed(range(layout.count())): - item = layout.itemAt(i) - - if isinstance(item, QWidgetItem): - # print "widget" + str(item) - item.widget().close() - # or - # item.widget().setParent(None) - elif isinstance(item, QSpacerItem): - # print "spacer " + str(item) - continue - # no need to do extra stuff - else: - # print "layout " + str(item) - LayoutUtil.clear_layout(item.layout()) - - # remove the item from layout - layout.removeItem(item) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py deleted file mode 100644 index 3d6638f3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py +++ /dev/null @@ -1,137 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy -from python_qt_binding.QtGui import QStandardItem, QStandardItemModel -from .data_items import ReadonlyItem - - -class MessageTreeModel(QStandardItemModel): - - def __init__(self, parent=None): - # FIXME: why is this not working? should be the same as the following line... - #super(MessageTreeModel, self).__init__(parent) - QStandardItemModel.__init__(self, parent) - - def add_message(self, message_instance, message_name='', message_type='', message_path=''): - if message_instance is None: - return - self._recursive_create_items(self, message_instance, message_name, message_type, message_path) - - def _get_toplevel_items(self, index_list): - items = [self.itemFromIndex(index) for index in index_list] - uniqueItems = {} - for item in items: - while item.parent() is not None: - item = item.parent() - if item.row() not in uniqueItems: - uniqueItems[item.row()] = item - return uniqueItems.values() - - def _get_data_items_for_path(self, slot_name, slot_type_name, slot_path, **kwargs): - return (QStandardItem(slot_name), QStandardItem(slot_type_name), QStandardItem(slot_path)) - - def _recursive_create_items(self, parent, slot, slot_name, slot_type_name, slot_path, **kwargs): - row = [] - for item in self._get_data_items_for_path(slot_name, slot_type_name, slot_path, **kwargs): - item._path = slot_path - item._user_data = kwargs.get('user_data', None) - row.append(item) - - is_leaf_node = False - if hasattr(slot, '__slots__') and hasattr(slot, '_slot_types'): - for child_slot_name, child_slot_type in zip(slot.__slots__, slot._slot_types): - child_slot_path = slot_path + '/' + child_slot_name - child_slot = getattr(slot, child_slot_name) - self._recursive_create_items(row[0], child_slot, child_slot_name, child_slot_type, child_slot_path, **kwargs) - - elif type(slot) in (list, tuple) and (len(slot) > 0): - child_slot_type = slot_type_name[:slot_type_name.find('[')] - for index, child_slot in enumerate(slot): - child_slot_name = '[%d]' % index - child_slot_path = slot_path + child_slot_name - self._recursive_create_items(row[0], child_slot, child_slot_name, child_slot_type, child_slot_path, **kwargs) - - else: - is_leaf_node = True - - if parent is self and kwargs.get('top_level_row_number', None) is not None: - parent.insertRow(kwargs['top_level_row_number'], row) - else: - parent.appendRow(row) - - return (row, is_leaf_node) - - ''' - NOTE: I (Isaac Saito) suspect that this function might have same/similar - functionality with _recursive_create_items. - - @summary: Evaluate current node and the previous node on the same depth. - If the name of both nodes at the same depth is the same, - current name is added to the previous node. - If not, the current name gets added to the parent node. - At the end, this function calls itself recursively going down - 1 level deeper. - @param stditem_parent: QStandardItem. - @param names_on_branch: List of strings each of which - represents the name of the node. - Ex. If you have a hierarchy that looks like: - - /top_node/sub_node/subsub_node - - then this list would look like: - - [ top_node, sub_node, subsub_node ] - @author: Isaac Saito - ''' - @staticmethod - def _build_tree_recursive(stditem_parent, names_on_branch): - name_curr = names_on_branch.pop(0) - stditem_curr = ReadonlyItem(name_curr) - - row_index_parent = stditem_parent.rowCount() - 1 - # item at the bottom is your most recent node. - - name_prev = '' - stditem_prev = None - if not stditem_parent.child(row_index_parent) == None: - stditem_prev = stditem_parent.child(row_index_parent) - name_prev = stditem_prev.text() - - stditem = None - if not name_prev == name_curr: - stditem_parent.appendRow(stditem_curr) - stditem = stditem_curr - else: - stditem = stditem_prev - - rospy.logdebug('add_tree_node 1 name_curr=%s ' + '\n\t\t\t\t\tname_prev=%s row_index_parent=%d', name_curr, name_prev, row_index_parent) - if (0 < len(names_on_branch)): - MessageTreeModel._build_tree_recursive(stditem, names_on_branch) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py deleted file mode 100644 index 28919793..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Slot, QMimeData, QModelIndex, Qt, qWarning -from python_qt_binding.QtGui import QAction, QDrag, QHeaderView, QIcon, QMenu, QTreeView - - -class MessageTreeWidget(QTreeView): - - def __init__(self, parent=None): - super(MessageTreeWidget, self).__init__(parent) - self.setDragEnabled(True) - self.sortByColumn(0, Qt.AscendingOrder) - - self.header().setResizeMode(QHeaderView.ResizeToContents) - self.header().setContextMenuPolicy(Qt.CustomContextMenu) - self.header().customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested) - - self._action_item_expand = QAction(QIcon.fromTheme('zoom-in'), 'Expand Selected', self) - self._action_item_expand.triggered.connect(self._handle_action_item_expand) - self._action_item_collapse = QAction(QIcon.fromTheme('zoom-out'), 'Collapse Selected', self) - self._action_item_collapse.triggered.connect(self._handle_action_item_collapse) - self.customContextMenuRequested.connect(self.handle_customContextMenuRequested) - - def startDrag(self, supportedActions): - index = self.currentIndex() - if not index.isValid(): - return - - item = self.model().itemFromIndex(index) - path = getattr(item, '_path', None) - if path is None: - qWarning('MessageTreeWidget.startDrag(): no _path set on item %s' % item) - return - - data = QMimeData() - data.setText(item._path) - - drag = QDrag(self) - drag.setMimeData(data) - drag.exec_() - - @Slot('QPoint') - def handle_customContextMenuRequested(self, pos): - # show context menu - menu = QMenu(self) - self._context_menu_add_actions(menu, pos) - menu.exec_(self.mapToGlobal(pos)) - - def _context_menu_add_actions(self, menu, pos): - if self.selectionModel().hasSelection(): - menu.addAction(self._action_item_expand) - menu.addAction(self._action_item_collapse) - - def _handle_action_item_collapse(self): - self._handle_action_set_expanded(False) - - def _handle_action_item_expand(self): - self._handle_action_set_expanded(True) - - def _handle_action_set_expanded(self, expanded): - def recursive_set_expanded(index): - if index != QModelIndex(): - self.setExpanded(index, expanded) - recursive_set_expanded(index.child(0, 0)) - for index in self.selectedIndexes(): - recursive_set_expanded(index) - - @Slot('QPoint') - def handle_header_view_customContextMenuRequested(self, pos): - - # create context menu - menu = QMenu(self) - - action_toggle_auto_resize = menu.addAction('Auto-Resize') - action_toggle_auto_resize.setCheckable(True) - auto_resize_flag = (self.header().resizeMode(0) == QHeaderView.ResizeToContents) - action_toggle_auto_resize.setChecked(auto_resize_flag) - - action_toggle_sorting = menu.addAction('Sorting') - action_toggle_sorting.setCheckable(True) - action_toggle_sorting.setChecked(self.isSortingEnabled()) - - # show menu - action = menu.exec_(self.header().mapToGlobal(pos)) - - # evaluate user action - if action is action_toggle_auto_resize: - if auto_resize_flag: - self.header().setResizeMode(QHeaderView.Interactive) - else: - self.header().setResizeMode(QHeaderView.ResizeToContents) - - elif action is action_toggle_sorting: - self.setSortingEnabled(not self.isSortingEnabled()) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py deleted file mode 100644 index 2fb65767..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py +++ /dev/null @@ -1,135 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Open Source Robotics Foundation Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -import rospkg -import rospy - - -class PluginContainerWidget(QWidget): - """ - This widget accommodates a plugin widget that needs an area to show system - message. A plugin widget is the pane that provides plugin's main - functionalities. PluginContainerWidget visually encapsulates a plugin - widget. - - In order to print msg in the msg pane provided by this class, plugin widget - MUST define and emit following signals: - - - sig_sysmsg - - sig_progress - - Having said that this architecture is based on signals, it is recommended - that exceptions raised in classes that are used in a plugin widget be - aggregated in it, so that only plugin widget is responsible for emitting - signals. - """ - - def __init__(self, plugin_widget, - on_sys_msg=True, on_sysprogress_bar=True): - """ - @param plugin_widget: The main widget of an rqt plugin. - @type plugin_widget: QWidget - @type on_sys_msg: bool - @param on_sys_msg: If True, a pane that accommodates str messages will - appear in the plugin's region. - @param on_sysprogress_bar: If True, a progress bar will appear in the - plugin's region. - """ - super(PluginContainerWidget, self).__init__() - - ui_file = os.path.join(rospkg.RosPack().get_path('rqt_py_common'), - 'resource', 'plugin_container.ui') - loadUi(ui_file, self) - - self._plugin_widget = plugin_widget - self._splitter.insertWidget(0, self._plugin_widget) - self.setWindowTitle(self._plugin_widget.windowTitle()) - - # Default is on for these sys status widgets. Only if the flag for them - # are 'False', hide them. - if on_sys_msg: - self._plugin_widget.sig_sysmsg.connect(self.set_sysmsg) - else: - self._sysmsg_widget.hide() - - if on_sysprogress_bar: - self._plugin_widget.sig_sysprogress.connect(self.set_sysprogress) - else: - self._sysprogress_bar.hide() - - def set_sysprogress(self, sysprogress): - #TODO: Pass progress value - pass - - def set_sysmsg(self, sysmsg): - """ - Set system msg that's supposed to be shown in sys msg pane. - @type sysmsg: str - """ - rospy.loginfo('PluginContainerWidget; {}'.format(sysmsg)) - #self._sysmsg_widget.setPlainText(sysmsg) - self._sysmsg_widget.append(sysmsg) - - def shutdown(self): - - #TODO: Is shutdown step necessary for PluginContainerWidget? - - self._plugin_widget.shutdown() - - def save_settings(self, plugin_settings, instance_settings): - - #Save setting of PluginContainerWidget. - instance_settings.set_value('_splitter', self._splitter.saveState()) - - #Save setting of ContainED widget - self._plugin_widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - - # Restore the setting of PluginContainerWidget, separate from - # ContainED widget. - if instance_settings.contains('_splitter'): - self._splitter.restoreState(instance_settings.value('_splitter')) - else: - self._splitter.setSizes([100, 100, 200]) - - # Now restore the setting of ContainED widget - self._plugin_widget.restore_settings(plugin_settings, - instance_settings) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py deleted file mode 100644 index 768ba1fa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py +++ /dev/null @@ -1,848 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Author: Isaac Saito under the influence of rosmsg.__init__.py - -# TODO 3/11/2013 (Isaac) This is moved from rqt_action pkg due to circular -# dependency issue. This rosaction is supposed to be moved (pull requested) to -# actionlib in the future. - -""" -Modifying rosaction.__init__.py to add functionality for ROS Action. - -Implements rosaction command-line tools. - -The code API of the rosaction module is unstable (inheriting the status of -rosmsg) - -(2/4/2013) Most of codes are not tested with actinlib. There's -"#NOT_TESTED_FROM_HERE" sign in the code below. -""" - -from __future__ import print_function - -import collections -import inspect -import os -import sys -import yaml -from optparse import OptionParser - -import genmsg -# import genpy -import rosbag -import roslib -import roslib.message -import rospkg -import rospy - -class ROSActionException(Exception): pass -class ROSActionProtoException(Exception): pass -class RosActionProtoArgsException(Exception): pass - -# If flowtype chosen is default, instead use flow-style -# False except if meeting objects or arrays with more than -# this size of sub-elements -MAX_DEFAULT_NON_FLOW_ITEMS = 4 - -MODE_ACTION = '.action' - - -def rosaction_cmd_list(mode, full, argv=None): - if argv is None: - argv = sys.argv[1:] - parser = OptionParser(usage="usage: ros%s list" % mode[1:]) - options, args = parser.parse_args(argv[1:]) - if mode == MODE_ACTION: - subdir = 'action' - else: - raise ValueError('Unknown mode for iterate_packages: %s' % mode) - - rospack = rospkg.RosPack() - packs = sorted([x for x in iterate_packages(rospack, mode)]) - for (p, direc) in packs: - for file_ in _list_types(direc, subdir, mode): - rospy.loginfo("%s/%s" % (p, file_)) - - -def _get_action_class_genpy(type_str, message_type, reload_on_error=False): - """ - Taken from genpy.message._get_message_or_service_class - - Utility for retrieving message/service class instances. Used by - get_message_class and get_service_class. - :param type_str: 'msg' or 'srv', ``str`` - :param message_type: type name of message/service, ``str`` - :returns: Message/Service for message/service type or None, ``class`` - :raises: :exc:`ValueError` If message_type is invalidly specified - """ - # # parse package and local type name for import - package, base_type = genmsg.package_resource_name(message_type) - if not package: - if base_type == 'Header': - package = 'std_msgs' - else: - raise ValueError("message type is missing package name: %s" - % str(message_type)) - pypkg = val = None - try: - # import the package and return the class - pypkg = __import__('%s.%s' % (package, type_str)) - val = getattr(getattr(pypkg, type_str), base_type) - except ImportError: - val = None - except AttributeError: - val = None - -""" -Taken from genpy.message -cache for get_message_class -""" -_message_class_cache_genpy = {} - - -def get_message_class_genpy(message_type, reload_on_error=False): - """ - Taken from genpy.message.get_message_class - - Get the message class. NOTE: this function maintains a - local cache of results to improve performance. - :param message_type: type name of message, ``str`` - :param reload_on_error: (optional). Attempt to reload the Python - module if unable to load message the first time. Defaults to - False. This is necessary if messages are built after the first load. - :returns: Message class for message/service type, ``Message class`` - :raises :exc:`ValueError`: if message_type is invalidly specified - """ - if message_type in _message_class_cache_genpy: - return _message_class_cache_genpy[message_type] - cls = _get_action_class_genpy('action', message_type, - reload_on_error=reload_on_error) - if cls: - _message_class_cache_genpy[message_type] = cls - return cls - - -def _get_action_class(type_str, message_type, reload_on_error=False): - """ - Taken from roslib.message._get_message_or_service_class - """ - - # # parse package and local type name for import - package, base_type = genmsg.package_resource_name(message_type) - if not package: - if base_type == 'Header': - package = 'std_msgs' - else: - raise ValueError("message type is missing package name: %s" % - str(message_type)) - pypkg = val = None - try: - # bootstrap our sys.path - roslib.launcher.load_manifest(package) - - rospy.loginfo('package={} type_str={} base_type={}'.format( - package, type_str, base_type)) - - # import the package and return the class - pypkg = __import__('%s/%s' % (package, type_str)) - # pypkg = __import__('%s.%s'%(package, type_str)) - val = getattr(getattr(pypkg, type_str), base_type) - - except rospkg.ResourceNotFound: - val = None - rospy.loginfo('_get_action_class except 1') - except ImportError: - val = None - rospy.loginfo('_get_action_class except 2') - except AttributeError: - val = None - rospy.loginfo('_get_action_class except 3') - - # this logic is mainly to support rosh, so that a user doesn't - # have to exit a shell just because a message wasn't built yet - if val is None and reload_on_error: - try: - if pypkg: - reload(pypkg) - val = getattr(getattr(pypkg, type_str), base_type) - except: - val = None - return val - -""" -Taken from roslib.message -""" -# # cache for get_message_class -_action_class_cache = {} - - -def get_action_class(action_type, reload_on_error=False): - """ - Taken from roslib.message.get_action_class - """ - - if action_type in _action_class_cache: - return _action_class_cache[action_type] - # try w/o bootstrapping - cls = get_message_class_genpy(action_type, reload_on_error=reload_on_error) - if cls is None: - # try old loader w/ bootstrapping - cls = _get_action_class('action', action_type, - reload_on_error=reload_on_error) - if cls: - _action_class_cache[action_type] = cls - return cls - - -def iterate_packages(rospack, mode): - """ - Iterator for packages that contain actions - :param mode: .action, ``str`` - """ - if mode == MODE_ACTION: - subdir = 'action' - else: - raise ValueError('Unknown mode for iterate_packages: %s' % mode) - pkgs = rospack.list() - for p in pkgs: - d = os.path.join(rospack.get_path(p), subdir) - if os.path.isdir(d): - yield p, d - - -def _msg_filter(ext): - def mfilter(f): - """ - Predicate for filtering directory list. matches message files - :param f: filename, ``str`` - """ - return os.path.isfile(f) and f.endswith(ext) - return mfilter - - -def _list_resources(path, rfilter=os.path.isfile): - """ - List resources in a package directory within a particular - subdirectory. This is useful for listing messages, services, etc... - :param rfilter: resource filter function that returns true if filename is - the desired resource type, ``fn(filename)->bool`` - """ - resources = [] - if os.path.isdir(path): - resources = [f for f in os.listdir(path) if rfilter(os.path.join(path, f))] - else: - resources = [] - return resources - - -def _list_types(path, subdir, ext): - """ - List all messages in the specified package - :param package str: name of package to search - :param include_depends bool: if True, will also list messages in package - dependencies - :returns [str]: message type names - """ - types = _list_resources(path, _msg_filter(ext)) - result = [x[:-len(ext)] for x in types] - result.sort() - - rospy.loginfo('_list_types result={}'.format(result)) - - return result - - -def list_types(package, mode=MODE_ACTION): - """ - Lists msg/srvs contained in package - :param package: package name, ``str`` - :param mode: MODE_ACTION. Defaults to msgs, ``str`` - :returns: list of msgs/srv in package, ``[str]`` - """ - - rospack = rospkg.RosPack() - if mode == MODE_ACTION: - subdir = 'action' - else: - raise ValueError('Unknown mode for list_types: %s' % mode) - path = os.path.join(rospack.get_path(package), subdir) - - rospy.loginfo('list_types package={} mode={} path={}'.format(package, mode, - path)) - - return [genmsg.resource_name(package, t) - for t in _list_types(path, subdir, mode)] - - -def list_actions(package): - """ - List actions contained in package - :param package: package name, ``str`` - :returns: list of actions in package, ``[str]`` - """ - return list_types(package, mode=MODE_ACTION) - - -def rosactionmain(mode=MODE_ACTION): - """ - Main entry point for command-line tools (rosaction). - - rosaction can interact with either ros messages or ros services. The mode - param indicates which - :param mode: MODE_ACTION or MODE_SRV, ``str`` - """ - try: - if mode == MODE_ACTION: - ext, full = mode, "message type" - else: - raise ROSActionException("Invalid mode: %s" % mode) - - if len(sys.argv) == 1: - rospy.loginfo(fullusage('ros' + mode[1:])) - sys.exit(0) - - command = sys.argv[1] -# if command == 'show': -# rosaction_cmd_show(ext, full) -# elif command == 'package': -# rosaction_cmd_package(ext, full) -# elif command == 'packages': -# rosaction_cmd_packages(ext, full) - if command == 'list': - rosaction_cmd_list(ext, full) -# elif command == 'md5': -# rosaction_cmd_md5(ext, full) - elif command == '--help': - print(fullusage('ros' + mode[1:])) - sys.exit(0) - else: - print(fullusage('ros' + mode[1:])) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except KeyError as e: - print("Unknown message type: %s" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except rospkg.ResourceNotFound as e: - print("Invalid package: %s" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except ValueError as e: - print("Invalid type: '%s'" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except ROSActionException as e: - print(str(e), file=sys.stderr) - sys.exit(1) - except KeyboardInterrupt: - pass - -""" -"#NOT_TESTED_FROM_HERE"---------------------------------------- -From here are what are copied from __init__.py that I don't know yet -if they are necessary/useful. -""" - ## copied from the web, recipe for ordered yaml output ###### -def construct_ordered_mapping(self, node, deep=False): - if not isinstance(node, yaml.MappingNode): - raise yaml.constructor.ConstructorError(None, None, - "expected a mapping node, but found %s" % node.id, - node.start_mark) - mapping = collections.OrderedDict() - for key_node, value_node in node.value: - key = self.construct_object(key_node, deep=deep) - if not isinstance(key, collections.Hashable): - raise yaml.constructor.ConstructorError("while constructing a mapping", - node.start_mark, - "found unhashable key", key_node.start_mark) - value = self.construct_object(value_node, deep=deep) - mapping[key] = value - return mapping - -def construct_yaml_map_with_ordered_dict(self, node): - data = collections.OrderedDict() - yield data - value = self.construct_mapping(node) - data.update(value) - -def represent_ordered_mapping(self, tag, mapping, flow_style=None): - value = [] - node = yaml.MappingNode(tag, value, flow_style=flow_style) - if self.alias_key is not None: - self.represented_objects[self.alias_key] = node - best_style = True - if hasattr(mapping, 'items'): - mapping = list(mapping.items()) - for item_key, item_value in mapping: - node_key = self.represent_data(item_key) - node_value = self.represent_data(item_value) - if not (isinstance(node_key, yaml.ScalarNode) and not node_key.style): - best_style = False - if not (isinstance(node_value, yaml.ScalarNode) and not node_value.style): - best_style = False - value.append((node_key, node_value)) - if flow_style is None: - if self.default_flow_style is not None: - node.flow_style = self.default_flow_style - else: - node.flow_style = best_style - return node - -## end recipe for ordered yaml output ###### - - -def get_array_type_instance(field_type, default_package=None): - """ - returns a single instance of field_type, where field_type can be a - message or ros primitive or an flexible size array. - """ - field_type = field_type.strip().rstrip("[]") - if field_type == "empty": - return None - if not "/" in field_type: - # is either built-in, Header, or in same package - # it seems built-in types get a priority - if field_type in ['byte', 'int8', 'int16', 'int32', 'int64', \ - 'char', 'uint8', 'uint16', 'uint32', 'uint64']: - return 0 - elif field_type in ['float32', 'float64']: - return 0 - elif field_type in ['string']: - return "" - elif field_type == 'bool': - return False - elif field_type == 'time': - field_type = "std_msgs/Time" - elif field_type == 'duration': - field_type = "std_msgs/Duration" - elif field_type == 'Header': - field_type = "std_msgs/Header" - else: - if default_package is None: - return None - field_type = default_package + "/" + field_type - msg_class = roslib.message.get_message_class(field_type) - if (msg_class == None): - # not important enough to raise exception? - return None - instance = msg_class() - return instance - -def get_yaml_for_msg(msg, prefix='', time_offset=None, current_time=None, - field_filter=None, flow_style_=None, fill_arrays_=False): - """ - Builds a YAML string of message. - @param msg: A object, dict or array - @param flow_style_: if True, produces one line with brackets, if false uses multiple lines with indentation, if None uses both using heuristics - @param prefix: prefixes all lines with this string - @param fill_arrays_: if True, for all flexible size arrays an element will be generated - @param current_time: currently not used. Only provided for API compatibility. current_time passes in the current time with respect to the message. - @type current_time: Time - @param field_filter: filter the fields that are strified for Messages. - @type field_filter: fn(Message)->iter(str) - @type flow_style_: bool - @return: a string - """ - def object_representer(dumper, obj): - ndict = collections.OrderedDict() - index = 0 - # allow caller to select which fields of message are strified - if field_filter != None: - fields = list(field_filter(obj)) - else: - fields = obj.__slots__ - for key in fields: - if not key.startswith('_'): - val = getattr(obj, key) - if type(val) == list and len(val) > MAX_DEFAULT_NON_FLOW_ITEMS: - dumper.default_flow_style = flow_style_ - if time_offset is not None and isinstance(val, Time): - ndict[key] = val - time_offset - # create initial array element (e.g. for code completion) - elif fill_arrays_ == True and val == []: - message_type = obj._slot_types[index] - if (obj._type != None) and "/" in obj._type: - def_pack = obj._type.split("/")[0] - instance = get_array_type_instance(message_type, default_package=def_pack) - if instance == None: - # not important enough to raise exception? - ndict[key] = val - else: - ndict[key] = [instance] - elif not inspect.ismethod(val) and not inspect.isfunction(val): - ndict[key] = val - index += 1 - # as a hack, we improve the heuristics of pyyaml and say with less than 5 objects, no need for brackets - if len(ndict) > MAX_DEFAULT_NON_FLOW_ITEMS: - dumper.default_flow_style = flow_style_ - return dumper.represent_dict(ndict) - yaml.representer.SafeRepresenter.add_representer(None, object_representer) - - # we force False over None here and set the style in dumper, to - # avoid unecessary outer brackets pyyaml chooses e.g. to - # represent msg Int32 as "{data: 0}" - initial_flow_style = False - if flow_style_ == True: - initial_flow_style = True - - # need to set default flow style True for bash prototype - # means will generate one line with [] and {} brackets - # bash needs bracket notation for rostopic pub - txt = yaml.safe_dump(msg, - # None, True, False (None chooses a compromise) - default_flow_style=initial_flow_style, - # Can be None, '', '\'', '"', '|', '>'. - default_style='', - # indent=2, #>=2, indentation depth - # line_break=?, - # allow_unicode=?, - # if true writes plenty of tags - # canonical = False, - # version={}?, - # width=40, - # encoding=?, - # tags={}?, - # when True, produces --- at start - # explicit_start=False, - # when True, produces ... at end - # explicit_end=False - ) - if prefix != None and prefix != '': - result = prefix + ("\n" + prefix).join(txt.splitlines()) - else: - result = txt.rstrip('\n') - return result - - -def create_names_filter(names): - """ - returns a function to use as filter that returns all objects slots except those with names in list. - """ - return lambda obj : filter(lambda slotname : not slotname in names, obj.__slots__) - - -def init_rosaction_proto(): - if "OrderedDict" in collections.__dict__: - yaml.constructor.BaseConstructor.construct_mapping = construct_ordered_mapping - yaml.constructor.Constructor.add_constructor( - 'tag:yaml.org,2002:map', - construct_yaml_map_with_ordered_dict) - - yaml.representer.BaseRepresenter.represent_mapping = represent_ordered_mapping - yaml.representer.Representer.add_representer(collections.OrderedDict, - yaml.representer.SafeRepresenter.represent_dict) - -def rosaction_cmd_prototype(args): - init_rosaction_proto() - parser = OptionParser(usage="usage: rosactionproto msg/srv [options]", - description="Produces output or a msg or service request, intended for tab completion support.") - parser.add_option("-f", "--flow_style", - dest="flow_style", type="int", default=None, action="store", - help="if 1 always use brackets, if 0 never use brackets. Default is a heuristic mix.") - parser.add_option("-e", "--empty-arrays", - dest="empty_arrays", default=False, action="store_true", - help="if true flexible size arrays are not filled with default instance") - parser.add_option("-s", "--silent", - dest="silent", default=False, action="store_true", - help="if true supresses all error messages") - parser.add_option("-p", "--prefix", metavar="prefix", default="", - help="prefix to print before each line, can be used for indent") - parser.add_option("-H", "--no-hyphens", - dest="no_hyphens", default="", action="store_true", - help="if true output has no outer hyphens") - parser.add_option("-x", "--exclude-slots", metavar="exclude_slots", default="", - help="comma separated list of slot names not to print") - - options, args = parser.parse_args(args) - - try: - if len(args) < 2: - raise RosActionProtoArgsException("Insufficient arguments") - mode = ".%s" % args[0] - message_type = args[1] - field_filter = None - if options.exclude_slots != None and options.exclude_slots.strip() != "": - field_filter = create_names_filter(options.exclude_slots.split(',')) - - # possible extentions: options for - # - target language - # - initial values for standard types - # - get partial message (subtree) - - # try to catch the user specifying code-style types and error - if '::' in message_type: - if not options.silent: - parser.error("rosactionproto does not understand C++-style namespaces (i.e. '::').\nPlease refer to msg/srv types as 'package_name/Type'.") - elif '.' in message_type: - if not options.silent: - parser.error("invalid message type '%s'.\nPlease refer to msg/srv types as 'package_name/Type'." % message_type) - if not '/' in message_type: - # if only one such msg or srv exists, use it - results = [] - for found in rosaction_search(rospkg.RosPack(), mode, message_type): - results.append(found) - if len(results) > 1: - raise ROSActionProtoException("Ambiguous message name %s" % message_type) - elif len(results) < 1: - raise ROSActionProtoException("Unknown message name %s" % message_type) - else: - message_type = results[0] - - if mode == MODE_ACTION: - msg_class = roslib.message.get_message_class(message_type) - if (msg_class == None): - raise ROSActionProtoException("Unknown message class: %s" % message_type) - instance = msg_class() - else: - raise ROSActionProtoException("Invalid mode: %s" % mode) - txt = get_yaml_for_msg(instance, - prefix=options.prefix, - flow_style_=options.flow_style, - fill_arrays_=not options.empty_arrays, - field_filter=field_filter) - - if options.no_hyphens == True: - return txt - else: - return '"' + txt + '"' - - except KeyError as e: - if not options.silent: - sys.stderr.write("Unknown message type: %s" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - # except rospkg.InvalidROSPkgException as e: - # if not options.silent: - # print(file=sys.stderr, "Invalid package: '%s'"%e) - # sys.exit(getattr(os, 'EX_USAGE', 1)) - except ValueError, e: - if not options.silent: - sys.stderr.write("Invalid type: '%s'" % e) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except ROSActionProtoException as e: - if not options.silent: - sys.stderr.write(str(e)) - sys.exit(1) - except RosActionProtoArgsException as e: - if not options.silent: - sys.stderr.write("%s" % e) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except KeyboardInterrupt: - pass - -#### Start of rosmsg #### - -try: - from cStringIO import StringIO # Python 2.x -except ImportError: - from io import StringIO # Python 3.x -def spec_to_str(action_context, spec, buff=None, indent=''): - """ - Convert spec into a string representation. Helper routine for MsgSpec. - :param indent: internal use only, ``str`` - :param buff: internal use only, ``StringIO`` - :returns: string representation of spec, ``str`` - """ - if buff is None: - buff = StringIO() - for c in spec.constants: - buff.write("%s%s %s=%s\n" % (indent, c.type, c.name, c.val_text)) - for type_, name in zip(spec.types, spec.names): - buff.write("%s%s %s\n" % (indent, type_, name)) - base_type = genmsg.msgs.bare_msg_type(type_) - if not base_type in genmsg.msgs.BUILTIN_TYPES: - subspec = msg_context.get_registered(base_type) - spec_to_str(msg_context, subspec, buff, indent + ' ') - return buff.getvalue() - -def get_msg_text(type_, raw=False, rospack=None): - """ - Get .msg file for type_ as text - :param type_: message type, ``str`` - :param raw: if True, include comments and whitespace (default False), ``bool`` - :returns: text of .msg file, ``str`` - :raises :exc:`ROSActionException` If type_ is unknown - """ - if rospack is None: - rospack = rospkg.RosPack() - search_path = {} - for p in rospack.list(): - search_path[p] = [os.path.join(rospack.get_path(p), 'msg')] - - context = genmsg.MsgContext.create_default() - try: - spec = genmsg.load_msg_by_type(context, type_, search_path) - genmsg.load_depends(context, spec, search_path) - except Exception as e: - raise ROSActionException("Unable to load msg [%s]: %s" % (type_, e)) - - if raw: - return spec.text - else: - return spec_to_str(context, spec) - -def _msg_filter(ext): - def mfilter(f): - """ - Predicate for filtering directory list. matches message files - :param f: filename, ``str`` - """ - return os.path.isfile(f) and f.endswith(ext) - return mfilter - - -def rosaction_search(rospack, mode, base_type): - """ - Iterator for all packages that contain a message matching base_type - - :param base_type: message base type to match, e.g. 'String' would match std_msgs/String, ``str`` - """ - for p, path in iterate_packages(rospack, mode): - if os.path.isfile(os.path.join(path, "%s%s" % (base_type, mode))): - yield genmsg.resource_name(p, base_type) - -def _stdin_arg(parser, full): - options, args = parser.parse_args(sys.argv[2:]) - # read in args from stdin pipe if not present - if not args: - arg = None - while not arg: - arg = sys.stdin.readline().strip() - return options, arg - else: - if len(args) > 1: - parser.error("you may only specify one %s" % full) - return options, args[0] - -def rosaction_cmd_show(mode, full): - cmd = "ros%s" % (mode[1:]) - parser = OptionParser(usage="usage: %s show [options] <%s>" % (cmd, full)) - parser.add_option("-r", "--raw", - dest="raw", default=False, action="store_true", - help="show raw message text, including comments") - parser.add_option("-b", "--bag", - dest="bag", default=None, - help="show message from .bag file", metavar="BAGFILE") - options, arg = _stdin_arg(parser, full) - if arg.endswith(mode): - arg = arg[:-(len(mode))] - - # try to catch the user specifying code-style types and error - if '::' in arg: - parser.error(cmd + " does not understand C++-style namespaces (i.e. '::').\nPlease refer to msg/srv types as 'package_name/Type'.") - elif '.' in arg: - parser.error("invalid message type '%s'.\nPlease refer to msg/srv types as 'package_name/Type'." % arg) - if options.bag: - bag_file = options.bag - if not os.path.exists(bag_file): - raise ROSActionException("ERROR: bag file [%s] does not exist" % bag_file) - for topic, msg, t in rosbag.Bag(bag_file).read_messages(raw=True): - datatype, _, _, _, pytype = msg - if datatype == arg: - print(get_msg_text(datatype, options.raw, pytype._full_text)) - break - else: - rospack = rospkg.RosPack() - if '/' in arg: # package specified - rosaction_debug(rospack, mode, arg, options.raw) - else: - for found in rosaction_search(rospack, mode, arg): - print("[%s]:" % found) - rosaction_debug(rospack, mode, found, options.raw) - -def rosaction_md5(mode, type_): - try: - if mode == MODE_ACTION: - msg_class = roslib.message.get_message_class(type_) - else: - msg_class = roslib.message.get_service_class(type_) - except ImportError: - raise IOError("cannot load [%s]" % (type_)) - if msg_class is not None: - return msg_class._md5sum - else: - raise IOError("cannot load [%s]" % (type_)) - -def rosaction_cmd_md5(mode, full): - parser = OptionParser(usage="usage: ros%s md5 <%s>" % (mode[1:], full)) - options, arg = _stdin_arg(parser, full) - - if '/' in arg: # package specified - try: - md5 = rosaction_md5(mode, arg) - print(md5) - except IOError: - print("Cannot locate [%s]" % arg, file=sys.stderr) - else: - rospack = rospkg.RosPack() - matches = [m for m in rosaction_search(rospack, mode, arg)] - for found in matches: - try: - md5 = rosaction_md5(mode, found) - print("[%s]: %s" % (found, md5)) - except IOError: - print("Cannot locate [%s]" % found, file=sys.stderr) - if not matches: - print("No messages matching the name [%s]" % arg, file=sys.stderr) - -def rosaction_cmd_package(mode, full): - parser = OptionParser(usage="usage: ros%s package " % mode[1:]) - parser.add_option("-s", - dest="single_line", default=False, action="store_true", - help="list all msgs on a single line") - options, arg = _stdin_arg(parser, full) - joinstring = '\n' - if options.single_line: - joinstring = ' ' - print(joinstring.join(list_types(arg, mode=mode))) - -def rosaction_cmd_packages(mode, full, argv=None): - if argv is None: - argv = sys.argv[1:] - parser = OptionParser(usage="usage: ros%s packages" % mode[1:]) - parser.add_option("-s", - dest="single_line", default=False, action="store_true", - help="list all packages on a single line") - options, args = parser.parse_args(argv[1:]) - rospack = rospkg.RosPack() - joinstring = '\n' - if options.single_line: - joinstring = ' ' - p1 = [p for p, _ in iterate_packages(rospack, mode)] - p1.sort() - print(joinstring.join(p1)) - -def rosaction_debug(rospack, mode, type_, raw=False): - """ - Prints contents of msg/srv file - :param mode: MODE_ACTION or MODE_SRV, ``str`` - """ - if mode == MODE_ACTION: - print(get_msg_text(type_, raw=raw, rospack=rospack)) - else: - raise ROSActionException("Invalid mode for debug: %s" % mode) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py deleted file mode 100644 index 87eae043..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py +++ /dev/null @@ -1,163 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from python_qt_binding.QtCore import Qt -import rospy - - -class RqtRosGraph(object): - - DELIM_GRN = '/' - - @staticmethod - def get_full_grn(model_index): - """ - @deprecated: Not completed. - - Create full path format of GRN (Graph Resource Names, see - http://www.ros.org/wiki/Names). Build GRN by recursively transcending - parents & children of a given QModelIndex instance. - - A complete example of GRN: /wide_stereo/left/image_color/compressed - - Upon its very 1st call, the argument is the index where user clicks on - on the view object (here QTreeView is used but should work with other - View too. Not tested yet though). str_grn can be 0-length string. - - :type model_index: QModelIndex - :type str_grn: str - :param str_grn: This could be an incomplete or a complete GRN format. - :rtype: str - """ - - children_grn_list = RqtRosGraph.get_lower_grn_dfs(model_index) - parent_data = model_index.data() - rospy.logdebug('parent_data={}'.format(parent_data)) - if parent_data == None: # model_index is 1st-order node of a tree. - upper_grn = RqtRosGraph.get_upper_grn(model_index, '') - grn_list = [] - for child_grn in children_grn_list: - grn_full = upper_grn + child_grn - rospy.logdebug('grn_full={} upper_grn={} child_grn={}'.format( - grn_full, upper_grn, child_grn)) - grn_list.append(grn_full) - else: - grn_list = children_grn_list - - #Create a string where namespace is delimited by slash. - grn = '' - for s in grn_list: - grn += RqtRosGraph.DELIM_GRN + s - - return grn - - @staticmethod - def get_lower_grn_dfs(model_index, grn_prev=''): - """ - Traverse all children treenodes and returns a list of "partial" - GRNs. Partial means that this method returns names under current level. - - Ex. Consider a tree like this: - - Root - |--TopitemA - | |--1 - | |--2 - | |--3 - | |--4 - | |--5 - | |--6 - | |--7 - |--TopitemB - - Re-formatted in GRN (omitting root): - - TopitemA/1/2/3/4 - TopitemA/1/2/3/5/6 - TopitemA/1/2/3/5/7 - TopitemB - - Might not be obvious from tree representation but there are 4 nodes as - GRN form suggests. - - (doc from here TBD) - - :type model_index: QModelIndex - :type grn_prev: str - :rtype: str[] - """ - i_child = 0 - list_grn_children_all = [] - while True: # Loop per child. - grn_curr = grn_prev + RqtRosGraph.DELIM_GRN + str( - model_index.data()) - child_qmindex = model_index.child(i_child, 0) - - if (not child_qmindex.isValid()): - rospy.logdebug('!! DEADEND i_child=#{} grn_curr={}'.format( - i_child, grn_curr)) - if i_child == 0: - # Only when the current node has no children, add current - # GRN to the returning list. - list_grn_children_all.append(grn_curr) - return list_grn_children_all - - rospy.logdebug('Child#{} grn_curr={}'.format(i_child, grn_curr)) - - list_grn_children = RqtRosGraph.get_lower_grn_dfs(child_qmindex, - grn_curr) - for child_grn in list_grn_children: - child_grn = (grn_prev + - (RqtRosGraph.DELIM_GRN + grn_curr) + - (RqtRosGraph.DELIM_GRN + child_grn)) - - list_grn_children_all = list_grn_children_all + list_grn_children - rospy.logdebug('111 lennodes={} list_grn_children={}'.format( - len(list_grn_children_all), list_grn_children)) - rospy.logdebug('122 list_grn_children_all={}'.format( - list_grn_children_all)) - i_child += 1 - return list_grn_children_all - - @staticmethod - def get_upper_grn(model_index, str_grn): - if model_index.data(Qt.DisplayRole) == None: - return str_grn - str_grn = (RqtRosGraph.DELIM_GRN + - str(model_index.data(Qt.DisplayRole)) + - str_grn) - rospy.logdebug('get_full_grn_recur out str=%s', str_grn) - return RqtRosGraph.get_upper_grn(model_index.parent(), str_grn) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py deleted file mode 100644 index c9c3313a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py +++ /dev/null @@ -1,215 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import os - -import genmsg -import roslaunch -from roslaunch import RLException -import rospkg -import rospy -import rostopic - - -class RqtRoscommUtil(object): - - @staticmethod - def load_parameters(config, caller_id): - """ - Load parameters onto the parameter server. - - Copied from ROSLaunchRunner. - - @type config: roslaunch.config.ROSLaunchConfig - @raise RLException: - """ - - # XMLRPC proxy for communicating with master, 'xmlrpclib.ServerProxy' - param_server = config.master.get() - - param = None - try: - # multi-call style xmlrpc - # According to API doc, get_multi() returns - # multicall XMLRPC proxy for communicating with master, - # "xmlrpclib.MultiCall" - param_server_multi = config.master.get_multi() - - # clear specified parameter namespaces - # #2468 unify clear params to prevent error - for param in roslaunch.launch._unify_clear_params( - config.clear_params): - if param_server.hasParam(caller_id, param)[2]: - param_server_multi.deleteParam(caller_id, param) - r = param_server_multi() - for code, msg, _ in r: - if code != 1: - raise RLException("Failed to clear parameter {}: ".format( - msg)) - except RLException: - raise - except Exception, e: - rospy.logerr("load_parameters: unable to set params " + - "(last param was [{}]): {}".format(param, e)) - raise # re-raise as this is fatal - - try: - # multi-call objects are not reusable - param_server_multi = config.master.get_multi() - for param in config.params.itervalues(): - # suppressing this as it causes too much spam - # printlog("setting parameter [%s]"%param.key) - param_server_multi.setParam(caller_id, param.key, param.value) - r = param_server_multi() - for code, msg, _ in r: - if code != 1: - raise RLException("Failed to set parameter: " + - "%s" % (msg)) - except RLException: - raise - except Exception, e: - print("load_parameters: unable to set params (last param was " + - "[%s]): %s" % (param, e)) - raise # re-raise as this is fatal - rospy.logdebug("... load_parameters complete") - - @staticmethod - def iterate_packages(subdir): - """ - Iterator for packages that contain the given subdir. - - This method is generalizing rosmsg.iterate_packages. - - @param subdir: eg. 'launch', 'msg', 'srv', 'action' - @type subdir: str - @raise ValueError: - """ - if subdir == None or subdir == '': - raise ValueError('Invalid package subdir = {}'.format(subdir)) - - rospack = rospkg.RosPack() - - pkgs = rospack.list() - rospy.logdebug('pkgs={}'.format(pkgs)) - for p in pkgs: - d = os.path.join(rospack.get_path(p), subdir) - rospy.logdebug('rospack dir={}'.format(d)) - if os.path.isdir(d): - yield p, d - - @staticmethod - def list_files(package, subdir, file_extension='.launch'): - """ - #TODO: Come up with better name of the method. - - Taken from rosmsg. - Lists files contained in the specified package - - @param package: package name, ``str`` - @param file_extension: Defaults to '.launch', ``str`` - :returns: list of msgs/srv in package, ``[str]`` - """ - if subdir == None or subdir == '': - raise ValueError('Invalid package subdir = {}'.format(subdir)) - - rospack = rospkg.RosPack() - - path = os.path.join(rospack.get_path(package), subdir) - - return [genmsg.resource_name(package, t) - for t in RqtRoscommUtil._list_types( - path, file_extension)] - - @staticmethod - def _list_types(path, ext): - """ - Taken from rosmsg - - List all messages in the specified package - :param package str: name of package to search - :param include_depends bool: if True, will also list messages in - package dependencies. - :returns [str]: message type names - """ - types = RqtRoscommUtil._list_resources(path, - RqtRoscommUtil._msg_filter(ext)) - - result = [x for x in types] - # result = [x[:-len(ext)] for x in types] # Remove extension - - result.sort() - return result - - @staticmethod - def _list_resources(path, rfilter=os.path.isfile): - """ - Taken from rosmsg._list_resources - - List resources in a package directory within a particular - subdirectory. This is useful for listing messages, services, etc... - :param rfilter: resource filter function that returns true if filename - is the desired resource type, ``fn(filename)->bool`` - """ - resources = [] - if os.path.isdir(path): - resources = [f for f - in os.listdir(path) if rfilter(os.path.join(path, f))] - else: - resources = [] - return resources - - @staticmethod - def _msg_filter(ext): - """ - Taken from rosmsg._msg_filter - """ - def mfilter(f): - """ - Predicate for filtering directory list. matches message files - :param f: filename, ``str`` - """ - return os.path.isfile(f) and f.endswith(ext) - return mfilter - - @staticmethod - def is_roscore_running(): - """ - @rtype: bool - """ - try: - # Checkif rosmaster is running or not. - rostopic.get_topic_class('/rosout') - return True - except rostopic.ROSTopicIOException as e: - return False diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py deleted file mode 100644 index f2eb145e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rospy - -from python_qt_binding.QtCore import qWarning - -from .message_tree_model import MessageTreeModel -from .tree_model_completer import TreeModelCompleter - - -class TopicCompleter(TreeModelCompleter): - - def __init__(self, parent=None): - super(TopicCompleter, self).__init__(parent) - self.setModel(MessageTreeModel()) - - def splitPath(self, path): - # to handle array subscriptions, e.g. /topic/field[1]/subfield[2] - # we need to separate array subscriptions by an additional / - return super(TopicCompleter,self).splitPath(path.replace('[','/[')) - - def update_topics(self): - self.model().clear() - topic_list = rospy.get_published_topics() - for topic_path, topic_type in topic_list: - topic_name = topic_path.strip('/') - message_class = roslib.message.get_message_class(topic_type) - if message_class is None: - qWarning('TopicCompleter.update_topics(): could not get message class for topic type "%s" on topic "%s"' % (topic_type, topic_path)) - continue - message_instance = message_class() - self.model().add_message(message_instance, topic_name, topic_type, topic_path) - - -if __name__ == '__main__': - import sys - from python_qt_binding.QtGui import QApplication, QComboBox, QLineEdit, QMainWindow, QTreeView, QVBoxLayout, QWidget - app = QApplication(sys.argv) - mw = QMainWindow() - widget = QWidget(mw) - layout = QVBoxLayout(widget) - - edit = QLineEdit() - edit_completer = TopicCompleter(edit) - #edit_completer.setCompletionMode(QCompleter.InlineCompletion) - edit.setCompleter(edit_completer) - - combo = QComboBox() - combo.setEditable(True) - combo_completer = TopicCompleter(combo) - #combo_completer.setCompletionMode(QCompleter.InlineCompletion) - combo.lineEdit().setCompleter(combo_completer) - - model_tree = QTreeView() - model_tree.setModel(combo_completer.model()) - model_tree.expandAll() - for column in range(combo_completer.model().columnCount()): - model_tree.resizeColumnToContents(column) - - completion_tree = QTreeView() - completion_tree.setModel(combo_completer.completionModel()) - completion_tree.expandAll() - for column in range(combo_completer.completionModel().columnCount()): - completion_tree.resizeColumnToContents(column) - - layout.addWidget(model_tree) - layout.addWidget(completion_tree) - layout.addWidget(edit) - layout.addWidget(combo) - layout.setStretchFactor(model_tree, 1) - widget.setLayout(layout) - mw.setCentralWidget(widget) - - mw.move(300, 0) - mw.resize(800, 900) - mw.show() - app.exec_() diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py deleted file mode 100644 index 31c7bd78..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py +++ /dev/null @@ -1,66 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rospy - - -class TopicDict(object): - - def __init__(self): - self.update_topics() - - def get_topics(self): - return self.topic_dict - - def update_topics(self): - self.topic_dict = {} - topic_list = rospy.get_published_topics() - for topic_name, topic_type in topic_list: - message = roslib.message.get_message_class(topic_type)() - self.topic_dict.update(self._recursive_create_field_dict(topic_name, message)) - - def _recursive_create_field_dict(self, topic_name, field): - field_dict = {} - field_dict[topic_name] = { - 'type': type(field), - 'children': {}, - } - - if hasattr(field, '__slots__'): - for slot_name in field.__slots__: - field_dict[topic_name]['children'].update(self._recursive_create_field_dict(slot_name, getattr(field, slot_name))) - - return field_dict - - -if __name__ == '__main__': - import pprint - pprint.pprint(TopicDict().get_topics()) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py deleted file mode 100644 index 7e27be54..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py +++ /dev/null @@ -1,115 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import roslib.msgs -from rostopic import get_topic_type -from python_qt_binding.QtCore import qDebug - -def get_type_class(type_name): - if roslib.msgs.is_valid_constant_type(type_name): - if type_name == 'string': - return str - elif type_name == 'bool': - return bool - else: - return type(roslib.msgs._convert_val(type_name, 0)) - else: - return roslib.message.get_message_class(type_name) - -def get_field_type(topic_name): - """ - Get the Python type of a specific field in the given registered topic. - If the field is an array, the type of the array's values are returned and the is_array flag is set to True. - This is a static type check, so it works for unpublished topics and with empty arrays. - - :param topic_name: name of field of a registered topic, ``str``, i.e. '/rosout/file' - :returns: field_type, is_array - """ - # get topic_type and message_evaluator - topic_type, real_topic_name, _ = get_topic_type(topic_name) - if topic_type is None: - #qDebug('topic_helpers.get_field_type(%s): get_topic_type failed' % (topic_name)) - return None, False - - message_class = roslib.message.get_message_class(topic_type) - if message_class is None: - qDebug('topic_helpers.get_field_type(%s): get_message_class(%s) failed' % (topic_name, topic_type)) - return None, False - - slot_path = topic_name[len(real_topic_name):] - return get_slot_type(message_class, slot_path) - - -def get_slot_type(message_class, slot_path): - """ - Get the Python type of a specific slot in the given message class. - If the field is an array, the type of the array's values are returned and the is_array flag is set to True. - This is a static type check, so it works for unpublished topics and with empty arrays. - - :param message_class: message class type, ``type``, usually inherits from genpy.message.Message - :param slot_path: path to the slot inside the message class, ``str``, i.e. 'header/seq' - :returns: field_type, is_array - """ - is_array = False - fields = [f for f in slot_path.split('/') if f] - for field_name in fields: - try: - field_name, _, field_index = roslib.msgs.parse_type(field_name) - except roslib.msgs.MsgSpecException: - return None, False - if field_name not in getattr(message_class, '__slots__', []): - #qDebug('topic_helpers.get_slot_type(%s, %s): field not found: %s' % (message_class, slot_path, field_name)) - return None, False - slot_type = message_class._slot_types[message_class.__slots__.index(field_name)] - slot_type, slot_is_array, _ = roslib.msgs.parse_type(slot_type) - is_array = slot_is_array and field_index is None - - message_class = get_type_class(slot_type) - return message_class, is_array - - -def is_slot_numeric(topic_name): - """ - Check is a slot in the given topic is numeric, or an array of numeric values. - This is a static type check, so it works for unpublished topics and with empty arrays. - - :param topic_name: name of field of a registered topic, ``str``, i.e. '/rosout/file' - :returns: is_numeric, is_array, description - """ - field_type, is_array = get_field_type(topic_name) - if field_type in (int, float): - if is_array: - message = 'topic "%s" is numeric array: %s[]' % (topic_name, field_type) - else: - message = 'topic "%s" is numeric: %s' % (topic_name, field_type) - return True, is_array, message - - return False, is_array, 'topic "%s" is NOT numeric: %s' % (topic_name, field_type) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py deleted file mode 100644 index 29b808ab..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py +++ /dev/null @@ -1,49 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rospy - -from .message_tree_model import MessageTreeModel - - -class TopicTreeModel(MessageTreeModel): - - def __init__(self, parent=None): - super(TopicTreeModel, self).__init__(parent) - self.refresh() - - def refresh(self): - self.clear() - topic_list = rospy.get_published_topics() - for topic_path, topic_type in topic_list: - topic_name = topic_path.strip('/') - message_instance = roslib.message.get_message_class(topic_type)() - self.add_message(message_instance, topic_name, topic_type, topic_path) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py deleted file mode 100644 index 8cedc387..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QCompleter - - -class TreeModelCompleter(QCompleter): - separator = '/' - - def __init__(self, parent=None): - super(TreeModelCompleter, self).__init__(parent) - - def splitPath(self, path): - path = path.lstrip(self.separator) - path_list = path.split(self.separator) - return path_list - - def pathFromIndex(self, index): - return self.model().itemFromIndex(index)._path diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg b/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg deleted file mode 100644 index 515ee2c8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg +++ /dev/null @@ -1 +0,0 @@ -Val[5] vals diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg b/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg deleted file mode 100644 index b0ae1084..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg +++ /dev/null @@ -1 +0,0 @@ -float64[5] floats diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py b/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py deleted file mode 100644 index a3ad2b15..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2015, Robert Haschke -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import unittest - -class TestMessageTreeModel(unittest.TestCase): - def test_path_names(self): - from rqt_py_common.message_tree_model import MessageTreeModel - from rqt_py_common.msg import Val, ArrayVal - m = MessageTreeModel() - m.add_message(ArrayVal()) - root = m.item(0).child(0) - self.assertEqual(root._path, '/vals') - for i in range(0,5): - child = root.child(i) - self.assertEqual(child._path, '/vals[%s]' % i) - child = child.child(0) - self.assertEqual(child._path, '/vals[%s]/floats' % i) - for j in range(0,5): - self.assertEqual(child.child(j)._path, '/vals[%s]/floats[%s]' % (i,j)) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py b/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py deleted file mode 100755 index 64756c46..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from python_qt_binding.QtGui import QStandardItem, QStandardItemModel -from rqt_reconfigure.rqt_ros_graph import RqtRosGraph - - -class TestRqtRosGraph(unittest.TestCase): - """ - :author: Isaac Saito - """ - - def setUp(self): - unittest.TestCase.setUp(self) - - self._model = QStandardItemModel() - node1 = QStandardItem('node1') - self._node1_1 = QStandardItem('node1_1') - self._node1_1_1 = QStandardItem('node1_1_1') - node1_1_2 = QStandardItem('node1_1_2') - node1_2 = QStandardItem('node1_2') - - node1.appendRow(self._node1_1) - self._node1_1.appendRow(self._node1_1_1) - self._node1_1.appendRow(node1_1_2) - node1.appendRow(node1_2) - - self._model.appendRow(node1) - - #node_list = [node1, node1_1, self._node1_1_1, node1_1_2, node1_2] - #self._model.appendRow(node_list) - - self._grn_node1_1_1 = '/node1/node1_1/node1_1_1' - self._len_lower_grn_node1_1 = 2 - - def tearDown(self): - unittest.TestCase.tearDown(self) - del self._model - - def test_get_upper_grn(self): - self.assertEqual(RqtRosGraph.get_upper_grn(self._node1_1_1.index(), ''), - self._grn_node1_1_1) - - def test_get_lower_grn_dfs(self): - self.assertEqual(len(RqtRosGraph.get_lower_grn_dfs( - self._node1_1.index(), - '')), - self._len_lower_grn_node1_1) - - def test_get_full_grn(self): - self.assertEqual(RqtRosGraph.get_full_grn(self._node1_1_1.index()), - self._grn_node1_1_1) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py b/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py deleted file mode 100755 index 792d17de..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil - - -class TestRqtRoscommUtil(unittest.TestCase): - """ - @author: Isaac Saito - """ - - def setUp(self): - unittest.TestCase.setUp(self) - - self._totalnum_pkg_contains_launch = 41 # Varies depending on system. - - def tearDown(self): - unittest.TestCase.tearDown(self) - #del self._model - - def test_iterate_packages(self): - """ - Not a very good test because the right answer that is hardcoded varies - depending on the system where this unittest runs. - """ - pkg_num_sum = 0 - for pkg in RqtRoscommUtil.iterate_packages('launch'): - pkg_num_sum += 1 - print 'pkg={}'.format(pkg) - - print pkg_num_sum - self.assertEqual(pkg_num_sum, self._totalnum_pkg_contains_launch) - - def test_list_files(self): - """ - Not a very good test because the right answer that is hardcoded varies - depending on the system where this unittest runs. - """ - file_num = 0 - pkg_name = 'pr2_moveit_config' - _totalnum_launches_pkg_contains = 15 - subdir = 'launch' - file_ext = '.launch' - files = RqtRoscommUtil.list_files(pkg_name, subdir, file_ext) - for file in files: - file_num += 1 - print 'file={}'.format(file) - - print file_num - self.assertEqual(file_num, _totalnum_launches_pkg_contains) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst deleted file mode 100644 index 90c3c900..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_py_console -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt deleted file mode 100644 index d3d1f7fa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_py_console) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_py_console - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox deleted file mode 100644 index b540ebdd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_py_console is a Python GUI plugin providing an interactive Python console. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/package.xml b/workspace/src/rqt_common_plugins/rqt_py_console/package.xml deleted file mode 100644 index 4111943f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - rqt_py_console - 0.3.13 - rqt_py_console is a Python GUI plugin providing an interactive Python console. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_py_console - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - qt_gui - qt_gui_py_common - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml b/workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml deleted file mode 100644 index 121e2eb1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin providing an interactive Python console. - - - - - folder - Plugins related to miscellaneous tools. - - - applications-python - A Python GUI plugin providing an interactive Python console. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui b/workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui deleted file mode 100644 index 12810f1c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui +++ /dev/null @@ -1,53 +0,0 @@ - - - PyConsole - - - - 0 - 0 - 276 - 212 - - - - PyConsole - - - - 0 - - - 0 - - - 0 - - - 3 - - - 0 - - - - - 0 - - - - - - - - - - - PyConsoleTextEdit - QTextEdit -
rqt_py_console.py_console_text_edit
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console b/workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console deleted file mode 100755 index 8f9bfcb8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/setup.py b/workspace/src/rqt_common_plugins/rqt_py_console/setup.py deleted file mode 100644 index 6ebf4557..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_py_console'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/__init__.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py deleted file mode 100644 index 5eb37dbe..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py +++ /dev/null @@ -1,103 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QVBoxLayout, QWidget -from qt_gui.plugin import Plugin -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog -from py_console_widget import PyConsoleWidget - -try: - from spyder_console_widget import SpyderConsoleWidget - _has_spyderlib = True -except ImportError: - _has_spyderlib = False - - -class PyConsole(Plugin): - """ - Plugin providing an interactive Python console - """ - def __init__(self, context): - super(PyConsole, self).__init__(context) - self.setObjectName('PyConsole') - - self._context = context - self._use_spyderlib = _has_spyderlib - self._console_widget = None - self._widget = QWidget() - self._widget.setLayout(QVBoxLayout()) - self._widget.layout().setContentsMargins(0, 0, 0, 0) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - self._context.add_widget(self._widget) - - def _switch_console_widget(self): - self._widget.layout().removeWidget(self._console_widget) - self.shutdown_console_widget() - - if _has_spyderlib and self._use_spyderlib: - self._console_widget = SpyderConsoleWidget(self._context) - self._widget.setWindowTitle('SpyderConsole') - else: - self._console_widget = PyConsoleWidget(self._context) - self._widget.setWindowTitle('PyConsole') - if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - - self._widget.layout().addWidget(self._console_widget) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('use_spyderlib', self._use_spyderlib) - - def restore_settings(self, plugin_settings, instance_settings): - self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true']) - self._switch_console_widget() - - def trigger_configuration(self): - options = [ - {'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib}, - {'title': 'PyConsole', 'description': 'Simple Python console.'}, - ] - dialog = SimpleSettingsDialog(title='PyConsole Options') - dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib)) - console_type = dialog.get_settings()[0] - new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib) - if self._use_spyderlib != new_use_spyderlib: - self._use_spyderlib = new_use_spyderlib - self._switch_console_widget() - - def shutdown_console_widget(self): - if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'): - self._console_widget.shutdown() - - def shutdown_plugin(self): - self.shutdown_console_widget() diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py deleted file mode 100644 index 4f933700..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py +++ /dev/null @@ -1,68 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import sys -from code import InteractiveInterpreter -from exceptions import SystemExit - -from python_qt_binding import QT_BINDING, QT_BINDING_VERSION -from python_qt_binding.QtCore import Qt, Signal - -from qt_gui_py_common.console_text_edit import ConsoleTextEdit - - -class PyConsoleTextEdit(ConsoleTextEdit): - _color_stdin = Qt.darkGreen - _multi_line_char = ':' - _multi_line_indent = ' ' - _prompt = ('>>> ', '... ') # prompt for single and multi line - exit = Signal() - - def __init__(self, parent=None): - super(PyConsoleTextEdit, self).__init__(parent) - - self._interpreter_locals = {} - self._interpreter = InteractiveInterpreter(self._interpreter_locals) - - self._comment_writer.write('Python %s on %s\n' % (sys.version.replace('\n', ''), sys.platform)) - self._comment_writer.write('Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) - - self._add_prompt() - - def update_interpreter_locals(self, newLocals): - self._interpreter_locals.update(newLocals) - - def _exec_code(self, code): - try: - self._interpreter.runsource(code) - except SystemExit: # catch sys.exit() calls, so they don't close the whole gui - self.exit.emit() diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py deleted file mode 100644 index 97beee5d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py +++ /dev/null @@ -1,54 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -import py_console_text_edit - - -class PyConsoleWidget(QWidget): - def __init__(self, context=None): - super(PyConsoleWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_py_console'), 'resource', 'py_console_widget.ui') - loadUi(ui_file, self, {'PyConsoleTextEdit': py_console_text_edit.PyConsoleTextEdit}) - self.setObjectName('PyConsoleWidget') - - my_locals = { - 'context': context - } - self.py_console.update_interpreter_locals(my_locals) - self.py_console.print_message('The variable "context" is set to the PluginContext of this plugin.') - self.py_console.exit.connect(context.close_plugin) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py deleted file mode 100644 index 7c86d285..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QFont - -from spyderlib.widgets.internalshell import InternalShell -from spyderlib.utils.module_completion import moduleCompletion - - -class SpyderConsoleWidget(InternalShell): - def __init__(self, context=None): - my_locals = { - 'context': context - } - super(SpyderConsoleWidget, self).__init__(namespace=my_locals) - self.setObjectName('SpyderConsoleWidget') - self.set_pythonshell_font(QFont('Mono')) - self.interpreter.restore_stds() - - def get_module_completion(self, objtxt): - """Return module completion list associated to object name""" - return moduleCompletion(objtxt) - - def run_command(self, *args): - self.interpreter.redirect_stds() - super(SpyderConsoleWidget, self).run_command(*args) - self.flush() - self.interpreter.restore_stds() - - def shutdown(self): - self.exit_interpreter() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst deleted file mode 100644 index 63c7145c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst +++ /dev/null @@ -1,125 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_reconfigure -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- -* Added refresh button to re-scan reconfigure server list -* Now retains functioning nodes when refreshing -* Contributors: Kei Okada, Scott K Logan - -0.3.11 (2015-04-30) -------------------- -* restore support for parameter groups (`#162 `_) -* fix background colors for dark themes (`#293 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix slider bar, add context menus for common operations (`#251 `_) -* fix bug in float range calculations (`#241 `_) -* remove experimental suffix from rqt_reconfigure (`#256 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* remove unnecessary margins to improve usability on small screens (`#228 `_) - -0.3.5 (2014-05-07) ------------------- -* numerous improvements and bug fixes (`#209 `_, `#210 `_) -* add option to open list of names from command line (`#214 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* mark rqt_launch and rqt_reconfigure as experimental (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix updating range limits (`#108 `_) -* fix layout quirks (`#150 `_) -* fix icon for closing a node (`#48 `_) -* fix handling of enum parameters with strings - -0.2.17 (2013-07-04) -------------------- -* Improvement; "GUI hangs for awhile or completely, when any one of nodes doesn't return any value" (`#81 `_) - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- -* Fix; Segmentation fault using integer slider (`#63 `_) - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- -* Improve performance significantly upon launch (`#45 `_) - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- -* Add feature to delete of shown nodes feature - -0.2.8 (2013-01-11) ------------------- -* Fix; No Interaction with Boolean values (`#2 `_) - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* renamed rqt_param to rqt_reconfigure (added missing file) -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt deleted file mode 100644 index b4ea6679..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_reconfigure) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_reconfigure - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml b/workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml deleted file mode 100644 index f126309d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - rqt_reconfigure - 0.3.13 - This rqt plugin succeeds former dynamic_reconfigure's GUI - (reconfigure_gui), and provides the way to view and edit the parameters - that are accessible via dynamic_reconfigure.
-
- (12/27/2012) In the future, arbitrary parameters that are not associated - with any nodes (which are not handled by dynamic_reconfigure) might - become handled. - However, currently as the name indicates, this pkg solely is dependent - on dynamic_reconfigure that allows access to only those params latched - to nodes. - - Scott K Logan - - BSD - - http://ros.org/wiki/rqt_reconfigure - https://github.com/ros-visualization/rqt_common_plugins/rqt_reconfigure - https://github.com/ros-visualization/rqt_common_plugins/issues - - Isaac Saito - Ze'ev Klapow - - catkin - - dynamic_reconfigure - rospy - rqt_console - rqt_gui - rqt_gui_py - rqt_py_common - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml b/workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml deleted file mode 100644 index bd1bcc50..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Python GUI to view and edit dynamic_reconfigure parameters. - - - - - folder - Plugins related to configuration. - - - accessories-text-editor - A Python GUI for viewing and editing dynamic_reconfigure parameters. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui deleted file mode 100644 index ac50dccb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui +++ /dev/null @@ -1,51 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 20 - - - - Param - - - - 0 - - - - - - - TextLabel - - - - - - - - 30 - 25 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui deleted file mode 100644 index ec2ee66d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui +++ /dev/null @@ -1,51 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 30 - - - - Param - - - - 0 - - - - - - - TextLabel - - - - - - - - 30 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui deleted file mode 100644 index 42da28e5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui +++ /dev/null @@ -1,108 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 20 - - - - Param - - - - 0 - - - - - - - param_name - - - - - - - - 30 - 0 - - - - min - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 130 - 0 - - - - 1 - - - Qt::Horizontal - - - - - - - - 30 - 0 - - - - max - - - - - - - - 0 - 0 - - - - - 75 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui deleted file mode 100644 index c23bf3e8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui +++ /dev/null @@ -1,51 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 20 - - - - Param string - - - - 0 - - - - - - - TextLabel - - - - - - - - 30 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui deleted file mode 100644 index 9a129107..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui +++ /dev/null @@ -1,148 +0,0 @@ - - - _node_selector_pane - - - - 0 - 0 - 581 - 479 - - - - - 0 - - - - - - - - 0 - 0 - - - - - 75 - 0 - - - - &Collapse all - - - - - - - - 0 - 0 - - - - - 75 - 0 - - - - - 50 - false - - - - &Expand all - - - false - - - false - - - false - - - false - - - - - - - - - - 0 - 0 - - - - QAbstractItemView::MultiSelection - - - true - - - true - - - false - - - - - - - - 0 - 0 - - - - - 75 - 0 - - - - - 50 - false - - - - &Refresh - - - false - - - false - - - false - - - false - - - - - - - - NodeSelectorWidget - QWidget -
rqt_reconfigure.node_selector_widget
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui deleted file mode 100644 index 4ba54196..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui +++ /dev/null @@ -1,58 +0,0 @@ - - - ParamWidget - - - - 0 - 0 - 513 - 430 - - - - - 0 - 0 - - - - - 300 - 200 - - - - Dynamic Reconfigure - - - - 0 - - - 0 - - - - - - 0 - 0 - - - - QFrame::Panel - - - Qt::Horizontal - - - 2 - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui deleted file mode 100644 index 7a8424ad..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui +++ /dev/null @@ -1,31 +0,0 @@ - - - ParamEditWidget - - - - 0 - 0 - 858 - 235 - - - - Param Edit - - - - - - 6 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui deleted file mode 100644 index 5a5d65ac..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui +++ /dev/null @@ -1,59 +0,0 @@ - - - _paramedit_widget - - - - 0 - 0 - 666 - 555 - - - - - 0 - 300 - - - - Param - - - - 0 - - - - - - - true - - - - - 0 - 0 - 644 - 533 - - - - - - - - - - - - ParameditWidget - QWidget -
rqt_reconfigure.paramedit_widget
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui deleted file mode 100644 index 97e31025..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui +++ /dev/null @@ -1,484 +0,0 @@ - - - _node_widget - - - - 0 - 0 - 488 - 391 - - - - - 0 - 0 - - - - - 300 - 30 - - - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 127 - 127 - 127 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 127 - 127 - 127 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 10 - - - - Dynamic Reconfigure - - - - - - true - - - - 75 - true - true - - - - nodename - - - Qt::AlignHCenter|Qt::AlignTop - - - - - - - - - - - GroupWidget - QWidget -
rqt_reconfigure.param_groups
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui deleted file mode 100644 index dc7156da..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui +++ /dev/null @@ -1,32 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure b/workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure deleted file mode 100755 index 24b15366..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main -from rqt_reconfigure.param_plugin import ParamPlugin - -plugin = 'rqt_reconfigure.param_plugin.ParamPlugin' -main = Main(filename=plugin) -sys.exit(main.main(sys.argv, standalone=plugin, plugin_argument_provider=ParamPlugin.add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py deleted file mode 100644 index 4e8baeeb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_reconfigure'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/__init__.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py deleted file mode 100644 index e6bc13e5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py +++ /dev/null @@ -1,104 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import rospy - -from .param_editors import EditorWidget -from .param_groups import GroupWidget, find_cfg -from .param_updater import ParamUpdater - - -class DynreconfClientWidget(GroupWidget): - """ - A wrapper of dynamic_reconfigure.client instance. - Represents a widget where users can view and modify ROS params. - """ - - def __init__(self, reconf, node_name): - """ - :type reconf: dynamic_reconfigure.client - :type node_name: str - """ - - group_desc = reconf.get_group_descriptions() - rospy.logdebug('DynreconfClientWidget.group_desc=%s', group_desc) - super(DynreconfClientWidget, self).__init__(ParamUpdater(reconf), - group_desc, node_name) - - self.setMinimumWidth(150) - - self.reconf = reconf - self.updater.start() - self.reconf.config_callback = self.config_callback - self._node_grn = node_name - - def get_node_grn(self): - - return self._node_grn - - def config_callback(self, config): - - #TODO: Think about replacing callback architecture with signals. - - if config: - # TODO: should use config.keys but this method doesnt exist - - names = [name for name, v in config.items()] - # v isn't used but necessary to get key and put it into dict. - rospy.logdebug('config_callback name={} v={}'.format(name, v)) - - for widget in self.editor_widgets: - if isinstance(widget, EditorWidget): - if widget.param_name in names: - rospy.logdebug('EDITOR widget.param_name=%s', - widget.param_name) - widget.update_value(config[widget.param_name]) - elif isinstance(widget, GroupWidget): - cfg = find_cfg(config, widget.param_name) - rospy.logdebug('GROUP widget.param_name=%s', - widget.param_name) - widget.update_group(cfg) - - def close(self): - self.reconf.close() - self.updater.stop() - - for w in self.editor_widgets: - w.close() - - self.deleteLater() - - def filter_param(self, filter_key): - #TODO impl - pass diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py deleted file mode 100644 index c4b6f5a0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py +++ /dev/null @@ -1,187 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QSortFilterProxyModel -import rospy - -from rqt_reconfigure.treenode_qstditem import TreenodeQstdItem - - -class FilterChildrenModel(QSortFilterProxyModel): - """ - Extending QSortFilterProxyModel, this provides methods to filter children - tree nodes. - - QSortFilterProxyModel filters top-down direction starting from the - top-level of tree, and once a node doesn't hit the query it gets disabled. - Filtering with this class reflects the result from the bottom node. - - Ex. - #TODO example needed here - """ - - # Emitted when parameters filtered. int indicates the order/index of - # params displayed. - sig_filtered = Signal(int) - - def __init__(self, parent): - super(FilterChildrenModel, self).__init__(parent) - - # :Key: Internal ID of QModelIndex of each treenode. - # :Value: TreenodeStatus - # self._treenodes = OrderedDict() - - self._parent = parent - self._toplv_parent_prev = None - - def filterAcceptsRow(self, src_row, src_parent_qmindex): - """ - Overridden. - - Terminology: - "Treenode" is deliberately used to avoid confusion with "Node" in ROS. - - :type src_row: int - :type src_parent_qmindex: QModelIndex - """ - rospy.logdebug('filerAcceptRow 1') - return self._filter_row_recur(src_row, src_parent_qmindex) - - def _filter_row_recur(self, src_row, src_parent_qmindex): - """ - :type src_row: int - :type src_parent_qmindex: QModelIndex - """ - _src_model = self.sourceModel() - curr_qmindex = _src_model.index(src_row, 0, src_parent_qmindex) - curr_qitem = _src_model.itemFromIndex(curr_qmindex) - - if isinstance(curr_qitem, TreenodeQstdItem): - # If selectable ROS Node, get GRN name - nodename_fullpath = curr_qitem.get_raw_param_name() - text_filter_target = nodename_fullpath - rospy.logdebug(' Nodename full={} '.format(nodename_fullpath)) - else: - # If ReadonlyItem, this means items are the parameters, not a part - # of node name. So, get param name. - text_filter_target = curr_qitem.data(Qt.DisplayRole) - - regex = self.filterRegExp() - pos_hit = regex.indexIn(text_filter_target) - if pos_hit >= 0: # Query hit. - rospy.logdebug('curr data={} row={} col={}'.format( - curr_qmindex.data(), - curr_qmindex.row(), - curr_qmindex.column())) - - # Set all subsequent treenodes True - rospy.logdebug(' FCModel.filterAcceptsRow src_row={}'.format( - src_row) + - ' parent row={} data={}'.format( - src_parent_qmindex.row(), - src_parent_qmindex.data()) + - ' filterRegExp={}'.format(regex)) - - # If the index is the terminal treenode, parameters that hit - # the query are displayed at the root tree. - _child_index = curr_qmindex.child(0, 0) - if ((not _child_index.isValid()) and - (isinstance(curr_qitem, TreenodeQstdItem))): - self._show_params_view(src_row, curr_qitem) - - # Once we find a treenode that hits the query, no need to further - # traverse since what this method wants to know with the given - # index is whether the given index is supposed to be shown or not. - # Thus, just return True here. - return True - - if not isinstance(curr_qitem, TreenodeQstdItem): - return False # If parameters, no need for recursive filtering. - - # Evaluate children recursively. - row_child = 0 - while True: - child_qmindex = curr_qmindex.child(row_child, 0) - if child_qmindex.isValid(): - flag = self._filter_row_recur(row_child, curr_qmindex) - if flag: - return True - else: - return False - row_child += 1 - return False - - def _show_params_view(self, src_row, curr_qitem): - """ - :type curr_qitem: QStandardItem - """ - - rospy.logdebug('_show_params_view data={}'.format( - curr_qitem.data(Qt.DisplayRole))) - curr_qitem.enable_param_items() - - def _get_toplevel_parent_recur(self, qmindex): - p = qmindex.parent() - if p.isValid(): - self._get_toplevel_parent(p) - return p - - def filterAcceptsColumn(self, source_column, source_parent): - """ - Overridden. - - Doing nothing really since columns are not in use. - - :type source_column: int - :type source_parent: QModelIndex - """ - rospy.logdebug('FCModel.filterAcceptsCol source_col={} '.format( - source_column) + 'parent col={} row={} data={}'.format( - source_parent.column(), source_parent.row(), source_parent.data())) - return True - - def set_filter(self, filter_): - self._filter = filter_ - - # If filtered text is '' (0-length str), invalidate current - # filtering, in the hope of making filtering process faster. - if filter_.get_text == '': - self.invalidate() - rospy.loginfo('filter invalidated.') - - # By calling setFilterRegExp, filterAccepts* methods get kicked. - self.setFilterRegExp(self._filter.get_regexp()) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py deleted file mode 100644 index 7dda8dc4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py +++ /dev/null @@ -1,477 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from collections import OrderedDict -import os -import time - -import dynamic_reconfigure as dyn_reconf -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import (QHeaderView, QItemSelectionModel, - QWidget) -import rospy -from rospy.exceptions import ROSException -import rosservice - -from rqt_py_common.rqt_ros_graph import RqtRosGraph -from rqt_reconfigure.filter_children_model import FilterChildrenModel -from rqt_reconfigure.treenode_qstditem import TreenodeQstdItem -from rqt_reconfigure.treenode_item_model import TreenodeItemModel - -from rqt_reconfigure.dynreconf_client_widget import DynreconfClientWidget - - -class NodeSelectorWidget(QWidget): - _COL_NAMES = ['Node'] - - # public signal - sig_node_selected = Signal(DynreconfClientWidget) - - def __init__(self, parent, rospack, signal_msg=None): - """ - @param signal_msg: Signal to carries a system msg that is shown on GUI. - @type signal_msg: QtCore.Signal - """ - super(NodeSelectorWidget, self).__init__() - self._parent = parent - self.stretch = None - self._signal_msg = signal_msg - - ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), 'resource', - 'node_selector.ui') - loadUi(ui_file, self) - - # List of the available nodes. Since the list should be updated over - # time and we don't want to create node instance per every update - # cycle, This list instance should better be capable of keeping track. - self._nodeitems = OrderedDict() - # Dictionary. 1st elem is node's GRN name, - # 2nd is TreenodeQstdItem instance. - # TODO: Needs updated when nodes list updated. - - # Setup treeview and models - self._item_model = TreenodeItemModel() - self._rootitem = self._item_model.invisibleRootItem() # QStandardItem - - self._nodes_previous = None - - # Calling this method updates the list of the node. - # Initially done only once. - self._update_nodetree_pernode() - - # TODO(Isaac): Needs auto-update function enabled, once another - # function that updates node tree with maintaining - # collapse/expansion state. http://goo.gl/GuwYp can be a - # help. - - self._collapse_button.pressed.connect( - self._node_selector_view.collapseAll) - self._expand_button.pressed.connect(self._node_selector_view.expandAll) - self._refresh_button.pressed.connect(self._refresh_nodes) - - # Filtering preparation. - self._proxy_model = FilterChildrenModel(self) - self._proxy_model.setDynamicSortFilter(True) - self._proxy_model.setSourceModel(self._item_model) - self._node_selector_view.setModel(self._proxy_model) - self._filterkey_prev = '' - - # This 1 line is needed to enable horizontal scrollbar. This setting - # isn't available in .ui file. - # Ref. http://stackoverflow.com/a/6648906/577001 - self._node_selector_view.header().setResizeMode( - 0, QHeaderView.ResizeToContents) - - # Setting slot for when user clicks on QTreeView. - self.selectionModel = self._node_selector_view.selectionModel() - # Note: self.selectionModel.currentChanged doesn't work to deselect - # a treenode as expected. Need to use selectionChanged. - self.selectionModel.selectionChanged.connect( - self._selection_changed_slot) - - def node_deselected(self, grn): - """ - Deselect the index that corresponds to the given GRN. - - :type grn: str - """ - - # Obtain the corresponding index. - qindex_tobe_deselected = self._item_model.get_index_from_grn(grn) - rospy.logdebug('NodeSelWidt node_deselected qindex={} data={}'.format( - qindex_tobe_deselected, - qindex_tobe_deselected.data(Qt.DisplayRole))) - - # Obtain all indices currently selected. - indexes_selected = self.selectionModel.selectedIndexes() - for index in indexes_selected: - grn_from_selectedindex = RqtRosGraph.get_upper_grn(index, '') - rospy.logdebug(' Compare given grn={} grn from selected={}'.format( - grn, grn_from_selectedindex)) - # If GRN retrieved from selected index matches the given one. - if grn == grn_from_selectedindex: - # Deselect the index. - self.selectionModel.select(index, QItemSelectionModel.Deselect) - - def node_selected(self, grn): - """ - Select the index that corresponds to the given GRN. - - :type grn: str - """ - - # Obtain the corresponding index. - qindex_tobe_selected = self._item_model.get_index_from_grn(grn) - rospy.logdebug('NodeSelWidt node_selected qindex={} data={}'.format( - qindex_tobe_selected, - qindex_tobe_selected.data(Qt.DisplayRole))) - - - # Select the index. - if qindex_tobe_selected: - self.selectionModel.select(qindex_tobe_selected, QItemSelectionModel.Select) - - def _selection_deselected(self, index_current, rosnode_name_selected): - """ - Intended to be called from _selection_changed_slot. - """ - self.selectionModel.select(index_current, QItemSelectionModel.Deselect) - - try: - reconf_widget = self._nodeitems[ - rosnode_name_selected].get_dynreconf_widget() - except ROSException as e: - raise e - - # Signal to notify other pane that also contains node widget. - self.sig_node_selected.emit(reconf_widget) - #self.sig_node_selected.emit(self._nodeitems[rosnode_name_selected]) - - def _selection_selected(self, index_current, rosnode_name_selected): - """Intended to be called from _selection_changed_slot.""" - rospy.logdebug('_selection_changed_slot row={} col={} data={}'.format( - index_current.row(), index_current.column(), - index_current.data(Qt.DisplayRole))) - - # Determine if it's terminal treenode. - found_node = False - for nodeitem in self._nodeitems.itervalues(): - name_nodeitem = nodeitem.data(Qt.DisplayRole) - name_rosnode_leaf = rosnode_name_selected[ - rosnode_name_selected.rfind(RqtRosGraph.DELIM_GRN) + 1:] - - # If name of the leaf in the given name & the name taken from - # nodeitem list matches. - if ((name_nodeitem == rosnode_name_selected) and - (name_nodeitem[name_nodeitem.rfind(RqtRosGraph.DELIM_GRN) + 1:] - == name_rosnode_leaf)): - - rospy.logdebug('terminal str {} MATCH {}'.format( - name_nodeitem, name_rosnode_leaf)) - found_node = True - break - if not found_node: # Only when it's NOT a terminal we deselect it. - self.selectionModel.select(index_current, - QItemSelectionModel.Deselect) - return - - # Only when it's a terminal we move forward. - - item_child = self._nodeitems[rosnode_name_selected] - item_widget = None - try: - item_widget = item_child.get_dynreconf_widget() - except ROSException as e: - raise e - rospy.logdebug('item_selected={} child={} widget={}'.format( - index_current, item_child, item_widget)) - self.sig_node_selected.emit(item_widget) - - # Show the node as selected. - #selmodel.select(index_current, QItemSelectionModel.SelectCurrent) - - def _selection_changed_slot(self, selected, deselected): - """ - Sends "open ROS Node box" signal ONLY IF the selected treenode is the - terminal treenode. - Receives args from signal QItemSelectionModel.selectionChanged. - - :param selected: All indexs where selected (could be multiple) - :type selected: QItemSelection - :type deselected: QItemSelection - """ - - ## Getting the index where user just selected. Should be single. - if not selected.indexes() and not deselected.indexes(): - rospy.logerr('Nothing selected? Not ideal to reach here') - return - - index_current = None - if selected.indexes(): - index_current = selected.indexes()[0] - elif len(deselected.indexes()) == 1: - # Setting length criteria as 1 is only a workaround, to avoid - # Node boxes on right-hand side disappears when filter key doesn't - # match them. - # Indeed this workaround leaves another issue. Question for - # permanent solution is asked here http://goo.gl/V4DT1 - index_current = deselected.indexes()[0] - - rospy.logdebug(' - - - index_current={}'.format(index_current)) - - rosnode_name_selected = RqtRosGraph.get_upper_grn(index_current, '') - - # If retrieved node name isn't in the list of all nodes. - if not rosnode_name_selected in self._nodeitems.keys(): - # De-select the selected item. - self.selectionModel.select(index_current, - QItemSelectionModel.Deselect) - return - - if selected.indexes(): - try: - self._selection_selected(index_current, rosnode_name_selected) - except ROSException as e: - #TODO: print to sysmsg pane - err_msg = e.message + '. Connection to node=' + \ - format(rosnode_name_selected) + ' failed' - self._signal_msg.emit(err_msg) - rospy.logerr(err_msg) - - elif deselected.indexes(): - try: - self._selection_deselected(index_current, - rosnode_name_selected) - except ROSException as e: - rospy.logerr(e.message) - #TODO: print to sysmsg pane - - def get_paramitems(self): - """ - :rtype: OrderedDict 1st elem is node's GRN name, - 2nd is TreenodeQstdItem instance - """ - return self._nodeitems - - def _update_nodetree_pernode(self): - """ - """ - - # TODO(Isaac): 11/25/2012 dynamic_reconfigure only returns params that - # are associated with nodes. In order to handle independent - # params, different approach needs taken. - try: - nodes = dyn_reconf.find_reconfigure_services() - except rosservice.ROSServiceIOException as e: - rospy.logerr("Reconfigure GUI cannot connect to master.") - raise e # TODO Make sure 'raise' here returns or finalizes func. - - if not nodes == self._nodes_previous: - i_node_curr = 1 - num_nodes = len(nodes) - elapsedtime_overall = 0.0 - for node_name_grn in nodes: - # Skip this grn if we already have it - if node_name_grn in self._nodeitems: - i_node_curr += 1 - continue - - time_siglenode_loop = time.time() - - ####(Begin) For DEBUG ONLY; skip some dynreconf creation -# if i_node_curr % 2 != 0: -# i_node_curr += 1 -# continue - #### (End) For DEBUG ONLY. #### - - # Instantiate QStandardItem. Inside, dyn_reconf client will - # be generated too. - treenodeitem_toplevel = TreenodeQstdItem( - node_name_grn, TreenodeQstdItem.NODE_FULLPATH) - _treenode_names = treenodeitem_toplevel.get_treenode_names() - - try: - treenodeitem_toplevel.connect_param_server() - except rospy.exceptions.ROSException as e: - rospy.logerr(e.message) - #Skip item that fails to connect to its node. - continue - #TODO: Needs to show err msg on GUI too. - - # Using OrderedDict here is a workaround for StdItemModel - # not returning corresponding item to index. - self._nodeitems[node_name_grn] = treenodeitem_toplevel - - self._add_children_treenode(treenodeitem_toplevel, - self._rootitem, _treenode_names) - - time_siglenode_loop = time.time() - time_siglenode_loop - elapsedtime_overall += time_siglenode_loop - - _str_progress = 'reconf ' + \ - 'loading #{}/{} {} / {}sec node={}'.format( - i_node_curr, num_nodes, round(time_siglenode_loop, 2), - round(elapsedtime_overall, 2), node_name_grn) - - # NOT a debug print - please DO NOT remove. This print works - # as progress notification when loading takes long time. - rospy.logdebug(_str_progress) - i_node_curr += 1 - - def _add_children_treenode(self, treenodeitem_toplevel, - treenodeitem_parent, child_names_left): - """ - Evaluate current treenode and the previous treenode at the same depth. - If the name of both nodes is the same, current node instance is - ignored (that means children will be added to the same parent). If not, - the current node gets added to the same parent node. At the end, this - function gets called recursively going 1 level deeper. - - :type treenodeitem_toplevel: TreenodeQstdItem - :type treenodeitem_parent: TreenodeQstdItem. - :type child_names_left: List of str - :param child_names_left: List of strings that is sorted in hierarchical - order of params. - """ - # TODO(Isaac): Consider moving this method to rqt_py_common. - - name_currentnode = child_names_left.pop(0) - grn_curr = treenodeitem_toplevel.get_raw_param_name() - stditem_currentnode = TreenodeQstdItem(grn_curr, - TreenodeQstdItem.NODE_FULLPATH) - - # item at the bottom is your most recent node. - row_index_parent = treenodeitem_parent.rowCount() - 1 - - # Obtain and instantiate prev node in the same depth. - name_prev = '' - stditem_prev = None - if treenodeitem_parent.child(row_index_parent): - stditem_prev = treenodeitem_parent.child(row_index_parent) - name_prev = stditem_prev.text() - - stditem = None - # If the name of both nodes is the same, current node instance is - # ignored (that means children will be added to the same parent) - if name_prev != name_currentnode: - stditem_currentnode.setText(name_currentnode) - - # Arrange alphabetically by display name - insert_index = 0 - while insert_index < treenodeitem_parent.rowCount() and treenodeitem_parent.child(insert_index).text() < name_currentnode: - insert_index += 1 - - treenodeitem_parent.insertRow(insert_index, stditem_currentnode) - stditem = stditem_currentnode - else: - stditem = stditem_prev - - if child_names_left: - # TODO: Model is closely bound to a certain type of view (treeview) - # here. Ideally isolate those two. Maybe we should split into 2 - # class, 1 handles view, the other does model. - self._add_children_treenode(treenodeitem_toplevel, stditem, - child_names_left) - else: # Selectable ROS Node. - #TODO: Accept even non-terminal treenode as long as it's ROS Node. - self._item_model.set_item_from_index(grn_curr, stditem.index()) - - def _prune_nodetree_pernode(self): - try: - nodes = dyn_reconf.find_reconfigure_services() - except rosservice.ROSServiceIOException as e: - rospy.logerr("Reconfigure GUI cannot connect to master.") - raise e # TODO Make sure 'raise' here returns or finalizes func. - - for i in reversed(range(0, self._rootitem.rowCount())): - candidate_for_removal = self._rootitem.child(i).get_raw_param_name() - if not candidate_for_removal in nodes: - rospy.logdebug('Removing {} because the server is no longer available.'.format( - candidate_for_removal)) - self._nodeitems[candidate_for_removal].disconnect_param_server() - self._rootitem.removeRow(i) - self._nodeitems.pop(candidate_for_removal) - - def _refresh_nodes(self): - self._prune_nodetree_pernode() - self._update_nodetree_pernode() - - def close_node(self): - rospy.logdebug(" in close_node") - # TODO(Isaac) Figure out if dynamic_reconfigure needs to be closed. - - def set_filter(self, filter_): - """ - Pass fileter instance to the child proxymodel. - :type filter_: BaseFilter - """ - self._proxy_model.set_filter(filter_) - - def _test_sel_index(self, selected, deselected): - """ - Method for Debug only - """ - #index_current = self.selectionModel.currentIndex() - src_model = self._item_model - index_current = None - index_deselected = None - index_parent = None - curr_qstd_item = None - if selected.indexes(): - index_current = selected.indexes()[0] - index_parent = index_current.parent() - curr_qstd_item = src_model.itemFromIndex(index_current) - elif deselected.indexes(): - index_deselected = deselected.indexes()[0] - index_parent = index_deselected.parent() - curr_qstd_item = src_model.itemFromIndex(index_deselected) - - if selected.indexes() > 0: - rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format( - index_current, index_parent, index_deselected, - index_current.data(Qt.DisplayRole), - index_parent.data(Qt.DisplayRole),) - + ' desel.d={} cur.item={}'.format( - None, # index_deselected.data(Qt.DisplayRole) - curr_qstd_item)) - elif deselected.indexes(): - rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format( - index_current, index_parent, index_deselected, - None, index_parent.data(Qt.DisplayRole)) + - ' desel.d={} cur.item={}'.format( - index_deselected.data(Qt.DisplayRole), - curr_qstd_item)) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py deleted file mode 100644 index 55196fc4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py +++ /dev/null @@ -1,446 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import math -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Signal -from python_qt_binding.QtGui import (QDoubleValidator, QIntValidator, QLabel, - QMenu, QWidget) -from decimal import Decimal -import rospkg -import rospy - -EDITOR_TYPES = { - 'bool': 'BooleanEditor', - 'str': 'StringEditor', - 'int': 'IntegerEditor', - 'double': 'DoubleEditor', -} - -# These .ui files are frequently loaded multiple times. Since file access -# costs a lot, only load each file once. -rp = rospkg.RosPack() -ui_bool = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_bool.ui') -ui_str = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_string.ui') -ui_num = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_number.ui') -ui_int = ui_num -ui_enum = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_enum.ui') - - -class EditorWidget(QWidget): - ''' - This class is abstract -- its child classes should be instantiated. - - There exist two kinds of "update" methods: - - _update_paramserver for Parameter Server. - - update_value for the value displayed on GUI. - ''' - - def __init__(self, updater, config): - ''' - @param updater: A class that extends threading.Thread. - @type updater: rqt_reconfigure.param_updater.ParamUpdater - ''' - - super(EditorWidget, self).__init__() - - self._updater = updater - self.param_name = config['name'] - self.param_default = config['default'] - self.param_description = config['description'] - - self.old_value = None - - self.cmenu = QMenu() - self.cmenu.addAction(self.tr('Set to Default')).triggered.connect(self._set_to_default) - - def _update_paramserver(self, value): - ''' - Update the value on Parameter Server. - ''' - if value != self.old_value: - self.update_configuration(value) - self.old_value = value - - def update_value(self, value): - ''' - To be implemented in subclass, but still used. - - Update the value that's displayed on the arbitrary GUI component - based on user's input. - - This method is not called from the GUI thread, so any changes to - QObjects will need to be done through a signal. - ''' - self.old_value = value - - def update_configuration(self, value): - self._updater.update({self.param_name: value}) - - def display(self, grid): - ''' - Should be overridden in subclass. - - :type grid: QFormLayout - ''' - self._paramname_label.setText(self.param_name) -# label_paramname = QLabel(self.param_name) -# label_paramname.setWordWrap(True) - self._paramname_label.setMinimumWidth(100) - grid.addRow(self._paramname_label, self) - self.setToolTip(self.param_description) - self._paramname_label.setToolTip(self.param_description) - self._paramname_label.contextMenuEvent = self.contextMenuEvent - - def close(self): - ''' - Should be overridden in subclass. - ''' - pass - - def _set_to_default(self): - self._update_paramserver(self.param_default) - - def contextMenuEvent(self, e): - self.cmenu.exec_(e.globalPos()) - - -class BooleanEditor(EditorWidget): - _update_signal = Signal(bool) - - def __init__(self, updater, config): - super(BooleanEditor, self).__init__(updater, config) - loadUi(ui_bool, self) - - # Initialize to default - self.update_value(config['default']) - - # Make checkbox update param server - self._checkbox.stateChanged.connect(self._box_checked) - - # Make param server update checkbox - self._update_signal.connect(self._checkbox.setChecked) - - def _box_checked(self, value): - self._update_paramserver(bool(value)) - - def update_value(self, value): - super(BooleanEditor, self).update_value(value) - self._update_signal.emit(value) - - -class StringEditor(EditorWidget): - _update_signal = Signal(str) - - def __init__(self, updater, config): - super(StringEditor, self).__init__(updater, config) - loadUi(ui_str, self) - - self._paramval_lineedit.setText(config['default']) - - # Update param server when cursor leaves the text field - # or enter is pressed. - self._paramval_lineedit.editingFinished.connect(self.edit_finished) - - # Make param server update text field - self._update_signal.connect(self._paramval_lineedit.setText) - - # Add special menu items - self.cmenu.addAction(self.tr('Set to Empty String')).triggered.connect(self._set_to_empty) - - def update_value(self, value): - super(StringEditor, self).update_value(value) - rospy.logdebug('StringEditor update_value={}'.format(value)) - self._update_signal.emit(value) - - def edit_finished(self): - rospy.logdebug('StringEditor edit_finished val={}'.format( - self._paramval_lineedit.text())) - self._update_paramserver(self._paramval_lineedit.text()) - - def _set_to_empty(self): - self._update_paramserver('') - - -class IntegerEditor(EditorWidget): - _update_signal = Signal(int) - - def __init__(self, updater, config): - super(IntegerEditor, self).__init__(updater, config) - loadUi(ui_int, self) - - # Set ranges - self._min = int(config['min']) - self._max = int(config['max']) - self._min_val_label.setText(str(self._min)) - self._max_val_label.setText(str(self._max)) - self._slider_horizontal.setRange(self._min, self._max) - - # TODO: Fix that the naming of _paramval_lineEdit instance is not - # consistent among Editor's subclasses. - self._paramval_lineEdit.setValidator(QIntValidator(self._min, - self._max, self)) - - # Initialize to default - self._paramval_lineEdit.setText(str(config['default'])) - self._slider_horizontal.setValue(int(config['default'])) - - # Make slider update text (locally) - self._slider_horizontal.sliderMoved.connect(self._slider_moved) - - # Make keyboard input change slider position and update param server - self._paramval_lineEdit.editingFinished.connect(self._text_changed) - - # Make slider update param server - # Turning off tracking means this isn't called during a drag - self._slider_horizontal.setTracking(False) - self._slider_horizontal.valueChanged.connect(self._slider_changed) - - # Make the param server update selection - self._update_signal.connect(self._update_gui) - - # Add special menu items - self.cmenu.addAction(self.tr('Set to Maximum')).triggered.connect(self._set_to_max) - self.cmenu.addAction(self.tr('Set to Minimum')).triggered.connect(self._set_to_min) - - def _slider_moved(self): - # This is a "local" edit - only change the text - self._paramval_lineEdit.setText(str( - self._slider_horizontal.sliderPosition())) - - def _text_changed(self): - # This is a final change - update param server - # No need to update slider... update_value() will - self._update_paramserver(int(self._paramval_lineEdit.text())) - - def _slider_changed(self): - # This is a final change - update param server - # No need to update text... update_value() will - self._update_paramserver(self._slider_horizontal.value()) - - def update_value(self, value): - super(IntegerEditor, self).update_value(value) - self._update_signal.emit(int(value)) - - def _update_gui(self, value): - # Block all signals so we don't loop - self._slider_horizontal.blockSignals(True) - # Update the slider value - self._slider_horizontal.setValue(value) - # Make the text match - self._paramval_lineEdit.setText(str(value)) - self._slider_horizontal.blockSignals(False) - - def _set_to_max(self): - self._update_paramserver(self._max) - - def _set_to_min(self): - self._update_paramserver(self._min) - - -class DoubleEditor(EditorWidget): - _update_signal = Signal(float) - - def __init__(self, updater, config): - super(DoubleEditor, self).__init__(updater, config) - loadUi(ui_num, self) - - # Handle unbounded doubles nicely - if config['min'] != -float('inf'): - self._min = float(config['min']) - self._min_val_label.setText(str(self._min)) - else: - self._min = -1e10000 - self._min_val_label.setText('-inf') - - if config['max'] != float('inf'): - self._max = float(config['max']) - self._max_val_label.setText(str(self._max)) - else: - self._max = 1e10000 - self._max_val_label.setText('inf') - - if config['min'] != -float('inf') and config['max'] != float('inf'): - self._func = lambda x: x - self._ifunc = self._func - else: - self._func = lambda x: math.atan(x) - self._ifunc = lambda x: math.tan(x) - - # If we have no range, disable the slider - self.scale = (self._func(self._max) - self._func(self._min)) - if self.scale <= 0: - self.scale = 0 - self.setDisabled(True) - else: - self.scale = 100 / self.scale - - # Set ranges - self._slider_horizontal.setRange(self._get_value_slider(self._min), - self._get_value_slider(self._max)) - self._paramval_lineEdit.setValidator(QDoubleValidator( - self._min, self._max, - 8, self)) - - # Initialize to defaults - self._paramval_lineEdit.setText(str(config['default'])) - self._slider_horizontal.setValue( - self._get_value_slider(config['default'])) - - # Make slider update text (locally) - self._slider_horizontal.sliderMoved.connect(self._slider_moved) - - # Make keyboard input change slider position and update param server - self._paramval_lineEdit.editingFinished.connect(self._text_changed) - - # Make slider update param server - # Turning off tracking means this isn't called during a drag - self._slider_horizontal.setTracking(False) - self._slider_horizontal.valueChanged.connect(self._slider_changed) - - # Make the param server update selection - self._update_signal.connect(self._update_gui) - - # Add special menu items - self.cmenu.addAction(self.tr('Set to Maximum')).triggered.connect(self._set_to_max) - self.cmenu.addAction(self.tr('Set to Minimum')).triggered.connect(self._set_to_min) - self.cmenu.addAction(self.tr('Set to NaN')).triggered.connect(self._set_to_nan) - - def _slider_moved(self): - # This is a "local" edit - only change the text - self._paramval_lineEdit.setText('{0:f}'.format(Decimal(str( - self._get_value_textfield())))) - - def _text_changed(self): - # This is a final change - update param server - # No need to update slider... update_value() will - self._update_paramserver(float(self._paramval_lineEdit.text())) - - def _slider_changed(self): - # This is a final change - update param server - # No need to update text... update_value() will - self._update_paramserver(self._get_value_textfield()) - - def _get_value_textfield(self): - '''@return: Current value in text field.''' - return self._ifunc(self._slider_horizontal.sliderPosition() / - self.scale) if self.scale else 0 - - def _get_value_slider(self, value): - ''' - @rtype: double - ''' - return int(round((self._func(value)) * self.scale)) - - def update_value(self, value): - super(DoubleEditor, self).update_value(value) - self._update_signal.emit(float(value)) - - def _update_gui(self, value): - # Block all signals so we don't loop - self._slider_horizontal.blockSignals(True) - # Update the slider value if not NaN - if not math.isnan(value): - self._slider_horizontal.setValue(self._get_value_slider(value)) - elif not math.isnan(self.param_default): - self._slider_horizontal.setValue(self._get_value_slider(self.param_default)) - # Make the text match - self._paramval_lineEdit.setText('{0:f}'.format(Decimal(str(value)))) - self._slider_horizontal.blockSignals(False) - - def _set_to_max(self): - self._update_paramserver(self._max) - - def _set_to_min(self): - self._update_paramserver(self._min) - - def _set_to_nan(self): - self._update_paramserver(float('NaN')) - - -class EnumEditor(EditorWidget): - _update_signal = Signal(int) - - def __init__(self, updater, config): - super(EnumEditor, self).__init__(updater, config) - - loadUi(ui_enum, self) - - try: - enum = eval(config['edit_method'])['enum'] - except: - rospy.logerr("reconfig EnumEditor) Malformed enum") - return - - # Setup the enum items - self.names = [item['name'] for item in enum] - self.values = [item['value'] for item in enum] - - items = ["%s (%s)" % (self.names[i], self.values[i]) - for i in range(0, len(self.names))] - - # Add items to the combo box - self._combobox.addItems(items) - - # Initialize to the default - self._combobox.setCurrentIndex(self.values.index(config['default'])) - - # Make selection update the param server - self._combobox.currentIndexChanged['int'].connect(self.selected) - - # Make the param server update selection - self._update_signal.connect(self._update_gui) - - # Bind the context menu - self._combobox.contextMenuEvent = self.contextMenuEvent - - def selected(self, index): - self._update_paramserver(self.values[index]) - - def update_value(self, value): - super(EnumEditor, self).update_value(value) - self._update_signal.emit(self.values.index(value)) - - def _update_gui(self, idx): - # Block all signals so we don't loop - self._combobox.blockSignals(True) - self._combobox.setCurrentIndex(idx) - self._combobox.blockSignals(False) - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py deleted file mode 100644 index b05bf241..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py +++ /dev/null @@ -1,329 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import time - -from python_qt_binding.QtCore import QSize, Qt, Signal, QMargins -from python_qt_binding.QtGui import (QFont, QFormLayout, QHBoxLayout, QIcon, - QGroupBox, QLabel, QPushButton, - QTabWidget, QVBoxLayout, QWidget) -import rospy - -# *Editor classes that are not explicitly used within this .py file still need -# to be imported. They are invoked implicitly during runtime. -from .param_editors import BooleanEditor, DoubleEditor, EditorWidget, \ - EDITOR_TYPES, EnumEditor, IntegerEditor, \ - StringEditor - -_GROUP_TYPES = { - '': 'BoxGroup', - 'collapse': 'CollapseGroup', - 'tab': 'TabGroup', - 'hide': 'HideGroup', - 'apply': 'ApplyGroup', -} - - -def find_cfg(config, name): - ''' - (Ze'ev) reaaaaallly cryptic function which returns the config object for - specified group. - ''' - cfg = None - for k, v in config.items(): - try: - if k.lower() == name.lower(): - cfg = v - return cfg - else: - try: - cfg = find_cfg(v, name) - if cfg: - return cfg - except Exception as exc: - raise exc - except AttributeError: - pass - except Exception as exc: - raise exc - return cfg - - -class GroupWidget(QWidget): - ''' - (Isaac's guess as of 12/13/2012) - This class bonds multiple Editor instances that are associated with - a single node as a group. - ''' - - # public signal - sig_node_disabled_selected = Signal(str) - sig_node_state_change = Signal(bool) - - def __init__(self, updater, config, nodename): - ''' - :param config: - :type config: Dictionary? defined in dynamic_reconfigure.client.Client - :type nodename: str - ''' - - super(GroupWidget, self).__init__() - self.state = config['state'] - self.param_name = config['name'] - self._toplevel_treenode_name = nodename - - # TODO: .ui file needs to be back into usage in later phase. -# ui_file = os.path.join(rp.get_path('rqt_reconfigure'), -# 'resource', 'singlenode_parameditor.ui') -# loadUi(ui_file, self) - - verticalLayout = QVBoxLayout(self) - verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0)) - - _widget_nodeheader = QWidget() - _h_layout_nodeheader = QHBoxLayout(_widget_nodeheader) - _h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0)) - - self.nodename_qlabel = QLabel(self) - font = QFont('Trebuchet MS, Bold') - font.setUnderline(True) - font.setBold(True) - - # Button to close a node. - _icon_disable_node = QIcon.fromTheme('window-close') - _bt_disable_node = QPushButton(_icon_disable_node, '', self) - _bt_disable_node.setToolTip('Hide this node') - _bt_disable_node_size = QSize(36, 24) - _bt_disable_node.setFixedSize(_bt_disable_node_size) - _bt_disable_node.pressed.connect(self._node_disable_bt_clicked) - - _h_layout_nodeheader.addWidget(self.nodename_qlabel) - _h_layout_nodeheader.addWidget(_bt_disable_node) - - self.nodename_qlabel.setAlignment(Qt.AlignCenter) - font.setPointSize(10) - self.nodename_qlabel.setFont(font) - grid_widget = QWidget(self) - self.grid = QFormLayout(grid_widget) - verticalLayout.addWidget(_widget_nodeheader) - verticalLayout.addWidget(grid_widget, 1) - # Again, these UI operation above needs to happen in .ui file. - - self.tab_bar = None # Every group can have one tab bar - self.tab_bar_shown = False - - self.updater = updater - - self.editor_widgets = [] - self._param_names = [] - - self._create_node_widgets(config) - - rospy.logdebug('Groups node name={}'.format(nodename)) - self.nodename_qlabel.setText(nodename) - - # Labels should not stretch - #self.grid.setColumnStretch(1, 1) - #self.setLayout(self.grid) - - def collect_paramnames(self, config): - pass - - def _create_node_widgets(self, config): - ''' - :type config: Dict? - ''' - i_debug = 0 - for param in config['parameters']: - begin = time.time() * 1000 - editor_type = '(none)' - - if param['edit_method']: - widget = EnumEditor(self.updater, param) - elif param['type'] in EDITOR_TYPES: - rospy.logdebug('GroupWidget i_debug=%d param type =%s', - i_debug, - param['type']) - editor_type = EDITOR_TYPES[param['type']] - widget = eval(editor_type)(self.updater, param) - - self.editor_widgets.append(widget) - self._param_names.append(param['name']) - - rospy.logdebug('groups._create_node_widgets num editors=%d', - i_debug) - - end = time.time() * 1000 - time_elap = end - begin - rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format( - editor_type, i_debug, time_elap)) - i_debug += 1 - - for name, group in config['groups'].items(): - if group['type'] == 'tab': - widget = TabGroup(self, self.updater, group, self._toplevel_treenode_name) - elif group['type'] in _GROUP_TYPES.keys(): - widget = eval(_GROUP_TYPES[group['type']])(self.updater, group, self._toplevel_treenode_name) - else: - widget = eval(_GROUP_TYPES[''])(self.updater, group, self._toplevel_treenode_name) - - self.editor_widgets.append(widget) - rospy.logdebug('groups._create_node_widgets ' + - 'name=%s', - name) - - for i, ed in enumerate(self.editor_widgets): - ed.display(self.grid) - - rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d', - len(self.editor_widgets)) - - def display(self, grid): - grid.addRow(self) - - def update_group(self, config): - if 'state' in config: - old_state = self.state - self.state = config['state'] - if self.state != old_state: - self.sig_node_state_change.emit(self.state) - - names = [name for name in config.keys()] - - for widget in self.editor_widgets: - if isinstance(widget, EditorWidget): - if widget.param_name in names: - widget.update_value(config[widget.param_name]) - elif isinstance(widget, GroupWidget): - cfg = find_cfg(config, widget.param_name) - widget.update_group(cfg) - - def close(self): - for w in self.editor_widgets: - w.close() - - def get_treenode_names(self): - ''' - :rtype: str[] - ''' - return self._param_names - - def _node_disable_bt_clicked(self): - rospy.logdebug('param_gs _node_disable_bt_clicked') - self.sig_node_disabled_selected.emit(self._toplevel_treenode_name) - - -class BoxGroup(GroupWidget): - def __init__(self, updater, config, nodename): - super(BoxGroup, self).__init__(updater, config, nodename) - - self.box = QGroupBox(self.param_name) - self.box.setLayout(self.grid) - - def display(self, grid): - grid.addRow(self.box) - - -class CollapseGroup(BoxGroup): - def __init__(self, updater, config, nodename): - super(CollapseGroup, self).__init__(updater, config, nodename) - self.box.setCheckable(True) - self.box.clicked.connect(self.click_cb) - self.sig_node_state_change.connect(self.box.setChecked) - - for child in self.box.children(): - if child.isWidgetType(): - self.box.toggled.connect(child.setShown) - - self.box.setChecked(self.state) - - def click_cb(self, on): - self.updater.update({'groups': {self.param_name: on}}) - - -class HideGroup(BoxGroup): - def __init__(self, updater, config, nodename): - super(HideGroup, self).__init__(updater, config, nodename) - self.box.setShown(self.state) - self.sig_node_state_change.connect(self.box.setShown) - - -class TabGroup(GroupWidget): - def __init__(self, parent, updater, config, nodename): - super(TabGroup, self).__init__(updater, config, nodename) - self.parent = parent - - if not self.parent.tab_bar: - self.parent.tab_bar = QTabWidget() - - self.wid = QWidget() - self.wid.setLayout(self.grid) - - parent.tab_bar.addTab(self.wid, self.param_name) - - def display(self, grid): - if not self.parent.tab_bar_shown: - grid.addRow(self.parent.tab_bar) - self.parent.tab_bar_shown = True - - def close(self): - super(TabGroup, self).close() - self.parent.tab_bar = None - self.parent.tab_bar_shown = False - - -class ApplyGroup(BoxGroup): - class ApplyUpdater: - def __init__(self, updater, loopback): - self.updater = updater - self.loopback = loopback - self._configs_pending = {} - - def update(self, config): - for name, value in config.items(): - self._configs_pending[name] = value - self.loopback(config) - - def apply_update(self): - self.updater.update(self._configs_pending) - self._configs_pending = {} - - def __init__(self, updater, config, nodename): - self.updater = ApplyGroup.ApplyUpdater(updater, self.update_group) - super(ApplyGroup, self).__init__(self.updater, config, nodename) - - self.button = QPushButton('Apply %s' % self.param_name) - self.button.clicked.connect(self.updater.apply_update) - - self.grid.addRow(self.button) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py deleted file mode 100644 index 5ca3c670..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py +++ /dev/null @@ -1,71 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from rqt_gui_py.plugin import Plugin -from rqt_py_common.plugin_container_widget import PluginContainerWidget - -from .param_widget import ParamWidget - - -class ParamPlugin(Plugin): - - def __init__(self, context): - """ - :type context: qt_gui.PluginContext - """ - - super(ParamPlugin, self).__init__(context) - self.setObjectName('Dynamic Reconfigure') - - self._plugin_widget = ParamWidget(context) - self._widget = PluginContainerWidget(self._plugin_widget, True, False) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.shutdown() - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_reconfigure plugin') - group.add_argument('node_name', nargs='*', default=[], help='Node(s) to open automatically') - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py deleted file mode 100644 index d71a18de..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py +++ /dev/null @@ -1,107 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import threading -import time - -import rospy - - -class ParamUpdater(threading.Thread): - ''' - Using dynamic_reconfigure that is passed in __init__, this thread updates - the Dynamic Reconfigure server's value. - - This class works for a single element in a single parameter. - ''' - - #TODO: Modify variable names to the ones that's more intuitive. - - def __init__(self, reconf): - """ - :type reconf: dynamic_reconfigure - """ - super(ParamUpdater, self).__init__() - self.setDaemon(True) - - self._reconf = reconf - self._condition_variable = threading.Condition() - self._configs_pending = {} - self._timestamp_last_pending = None - self._stop_flag = False - - def run(self): - _timestamp_last_commit = None - - rospy.logdebug(' ParamUpdater started') - - while not self._stop_flag: - if _timestamp_last_commit >= self._timestamp_last_pending: - with self._condition_variable: - rospy.logdebug(' ParamUpdater loop 1.1') - self._condition_variable.wait() - rospy.logdebug(' ParamUpdater loop 1.2') - rospy.logdebug(' ParamUpdater loop 2') - - if self._stop_flag: - return - - _timestamp_last_commit = time.time() - configs_tobe_updated = self._configs_pending.copy() - self._configs_pending = {} - - rospy.logdebug(' run last_commit={}, last_pend={}'.format( - _timestamp_last_commit, self._timestamp_last_pending)) - - try: - self._reconf.update_configuration(configs_tobe_updated) - except rospy.ServiceException as ex: - rospy.logdebug('Could not update configs due to {}'.format( - ex.value)) - except Exception as exc: - raise exc - - def update(self, config): - with self._condition_variable: - for name, value in config.items(): - self._configs_pending[name] = value - - self._timestamp_last_pending = time.time() - - self._condition_variable.notify() - - def stop(self): - self._stop_flag = True - with self._condition_variable: - self._condition_variable.notify() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py deleted file mode 100644 index 63218951..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py +++ /dev/null @@ -1,175 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -import rospkg -import sys - -from python_qt_binding.QtCore import Signal, QMargins -from python_qt_binding.QtGui import (QLabel, QHBoxLayout, QSplitter, - QVBoxLayout, QWidget, QItemSelectionModel) - -from rqt_reconfigure.node_selector_widget import NodeSelectorWidget -from rqt_reconfigure.paramedit_widget import ParameditWidget -from rqt_reconfigure.text_filter import TextFilter -from rqt_reconfigure.text_filter_widget import TextFilterWidget -import rospy - -class ParamWidget(QWidget): - _TITLE_PLUGIN = 'Dynamic Reconfigure' - - # To be connected to PluginContainerWidget - sig_sysmsg = Signal(str) - sig_sysprogress = Signal(str) - - # To make selections from CLA - sig_selected = Signal(str) - - def __init__(self, context, node=None): - """ - This class is intended to be called by rqt plugin framework class. - Currently (12/12/2012) the whole widget is splitted into 2 panes: - one on left allows you to choose the node(s) you work on. Right side - pane lets you work with the parameters associated with the node(s) you - select on the left. - - (12/27/2012) Despite the pkg name is changed to rqt_reconfigure to - reflect the available functionality, file & class names remain - 'param', expecting all the parameters will become handle-able. - """ - - super(ParamWidget, self).__init__() - self.setObjectName(self._TITLE_PLUGIN) - self.setWindowTitle(self._TITLE_PLUGIN) - - rp = rospkg.RosPack() - - #TODO: .ui file needs to replace the GUI components declaration - # below. For unknown reason, referring to another .ui files - # from a .ui that is used in this class failed. So for now, - # I decided not use .ui in this class. - # If someone can tackle this I'd appreciate. - _hlayout_top = QHBoxLayout(self) - _hlayout_top.setContentsMargins(QMargins(0, 0, 0, 0)) - self._splitter = QSplitter(self) - _hlayout_top.addWidget(self._splitter) - - _vlayout_nodesel_widget = QWidget() - _vlayout_nodesel_side = QVBoxLayout() - _hlayout_filter_widget = QWidget(self) - _hlayout_filter = QHBoxLayout() - self._text_filter = TextFilter() - self.filter_lineedit = TextFilterWidget(self._text_filter, rp) - self.filterkey_label = QLabel("&Filter key:") - self.filterkey_label.setBuddy(self.filter_lineedit) - _hlayout_filter.addWidget(self.filterkey_label) - _hlayout_filter.addWidget(self.filter_lineedit) - _hlayout_filter_widget.setLayout(_hlayout_filter) - self._nodesel_widget = NodeSelectorWidget(self, rp, self.sig_sysmsg) - _vlayout_nodesel_side.addWidget(_hlayout_filter_widget) - _vlayout_nodesel_side.addWidget(self._nodesel_widget) - _vlayout_nodesel_side.setSpacing(1) - _vlayout_nodesel_widget.setLayout(_vlayout_nodesel_side) - - reconf_widget = ParameditWidget(rp) - - self._splitter.insertWidget(0, _vlayout_nodesel_widget) - self._splitter.insertWidget(1, reconf_widget) - # 1st column, _vlayout_nodesel_widget, to minimize width. - # 2nd col to keep the possible max width. - self._splitter.setStretchFactor(0, 0) - self._splitter.setStretchFactor(1, 1) - - # Signal from paramedit widget to node selector widget. - reconf_widget.sig_node_disabled_selected.connect( - self._nodesel_widget.node_deselected) - # Pass name of node to editor widget - self._nodesel_widget.sig_node_selected.connect( - reconf_widget.show_reconf) - - if not node: - title = self._TITLE_PLUGIN - else: - title = self._TITLE_PLUGIN + ' %s' % node - self.setObjectName(title) - - #Connect filter signal-slots. - self._text_filter.filter_changed_signal.connect( - self._filter_key_changed) - - # Open any clients indicated from command line - self.sig_selected.connect(self._nodesel_widget.node_selected) - for rn in [rospy.resolve_name(c) for c in context.argv()]: - if rn in self._nodesel_widget.get_paramitems(): - self.sig_selected.emit(rn) - else: - rospy.logwarn('Could not find a dynamic reconfigure client named \'%s\'', str(rn)) - - def shutdown(self): - #TODO: Needs implemented. Trigger dynamic_reconfigure to unlatch - # subscriber. - pass - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('splitter', self._splitter.saveState()) - - def restore_settings(self, plugin_settings, instance_settings): - if instance_settings.contains('splitter'): - self._splitter.restoreState(instance_settings.value('splitter')) - else: - self._splitter.setSizes([100, 100, 200]) - - def get_filter_text(self): - """ - :rtype: QString - """ - return self.filter_lineedit.text() - - def _filter_key_changed(self): - self._nodesel_widget.set_filter(self._text_filter) - - #TODO: This method should be integrated into common architecture. I just - # can't think of how to do so within current design. - def emit_sysmsg(self, msg_str): - self.sig_sysmsg.emit(msg_str) - - -if __name__ == '__main__': - # main should be used only for debug purpose. - # This launches this QWidget as a standalone rqt gui. - from rqt_gui.main import Main - - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_reconfigure')) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py deleted file mode 100644 index aeb0c201..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py +++ /dev/null @@ -1,167 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import os -from collections import OrderedDict - -import dynamic_reconfigure.client -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QVBoxLayout, QWidget, QWidgetItem -from rqt_py_common.layout_util import LayoutUtil -import rospy - -from .dynreconf_client_widget import DynreconfClientWidget - - -class ParameditWidget(QWidget): - """ - This class represents a pane where parameter editor widgets of multiple - nodes are shown. In rqt_reconfigure, this pane occupies right half of the - entire visible area. - """ - - # public signal - sig_node_disabled_selected = Signal(str) - - def __init__(self, rospack): - """""" - super(ParameditWidget, self).__init__() - - ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), - 'resource', 'paramedit_pane.ui') - loadUi(ui_file, self, {'ParameditWidget': ParameditWidget}) - - self._dynreconf_clients = OrderedDict() - - # Adding the list of Items - self.vlayout = QVBoxLayout(self.scrollarea_holder_widget) - - #self._set_index_widgets(self.listview, paramitems_dict) # causes error - self.destroyed.connect(self.close) - - def _set_index_widgets(self, view, paramitems_dict): - """ - @deprecated: Causes error - """ - i = 0 - for p in paramitems_dict: - view.setIndexWidget(i, p) - i += 1 - - def show_reconf(self, dynreconf_widget): - """ - Callback when user chooses a node. - - @param dynreconf_widget: - """ - node_grn = dynreconf_widget.get_node_grn() - rospy.logdebug('ParameditWidget.show str(node_grn)=%s', str(node_grn)) - - if not node_grn in self._dynreconf_clients.keys(): - # Add dynreconf widget if there isn't already one. - - # Client gets renewed every time different node_grn was clicked. - - self._dynreconf_clients.__setitem__(node_grn, dynreconf_widget) - self.vlayout.addWidget(dynreconf_widget) - dynreconf_widget.sig_node_disabled_selected.connect( - self._node_disabled) - - else: # If there has one already existed, remove it. - self._remove_node(node_grn) - #LayoutUtil.clear_layout(self.vlayout) - - # Re-add the rest of existing items to layout. - #for k, v in self._dynreconf_clients.iteritems(): - # rospy.loginfo('added to layout k={} v={}'.format(k, v)) - # self.vlayout.addWidget(v) - - # Add color to alternate the rim of the widget. - LayoutUtil.alternate_color(self._dynreconf_clients.itervalues(), - [self.palette().background().color().lighter(125), - self.palette().background().color().darker(125)]) - - def close(self): - for dc in self._dynreconf_clients: - # Clear out the old widget - dc.close() - dc = None - - self._paramedit_scrollarea.deleteLater() - - def filter_param(self, filter_key): - """ - :type filter_key: - """ - - #TODO Pick nodes that match filter_key. - - #TODO For the nodes that are kept in previous step, call - # DynreconfWidget.filter_param for all of its existing - # instances. - pass - - def _remove_node(self, node_grn): - try: - i = self._dynreconf_clients.keys().index(node_grn) - except ValueError: - # ValueError occurring here means that the specified key is not - # found, most likely already removed, which is possible in the - # following situation/sequence: - # - # Node widget on ParameditWidget removed by clicking disable button - # --> Node deselected on tree widget gets updated - # --> Tree widget detects deselection - # --> Tree widget emits deselection signal, which is captured by - # ParameditWidget's slot. Thus reaches this method again. - return - - item = self.vlayout.itemAt(i) - if isinstance(item, QWidgetItem): - item.widget().close() - w = self._dynreconf_clients.pop(node_grn) - - rospy.logdebug('popped={} Len of left clients={}'.format( - w, len(self._dynreconf_clients))) - - def _node_disabled(self, node_grn): - rospy.logdebug('paramedit_w _node_disabled grn={}'.format(node_grn)) - - # Signal to notify other GUI components (eg. nodes tree pane) that - # a node widget is disabled. - self.sig_node_disabled_selected.emit(node_grn) - - # Remove the selected node widget from the internal list of nodes. - self._remove_node(node_grn) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py deleted file mode 100644 index fb86bfd0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py +++ /dev/null @@ -1,106 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from python_qt_binding.QtCore import QRegExp, Qt -from rqt_console.filters.message_filter import MessageFilter - - -class TextFilter(MessageFilter): - """ - Provides a filtering feature for text set by set_text. - - Inheriting rqt_console.filters.MessageFilter, this class provides timeout - effect to the input widget (eg. QLineEdit) that contains this class. - """ - - def __init__(self, qregexp=None): - super(TextFilter, self).__init__() - self._regexp = qregexp - - def test_message(self, text): - """ - Overridden. - - :param message: the message to be tested against the filters. - :type message: str. - :rtype: bool - """ - - _hit = False - - if (self.is_enabled() and - self._text != '' and - not self._qregexp == None # If None, init process isn't done yet - # and we can ignore the call to this - # method. - ): - pos_hit = self._qregexp.indexIn(text) - if pos_hit >= 0: - _hit = True - else: - _hit = False - return _hit - -# def set_regexp(self, qregexp): -# """ -# Setter for self._qregexp. Need not be used if _qregexp is already set -# via __init__. Calling this method overwrites the existing regex -# instance. -# -# Do not mix up self._qregexp with MessageFilter._regex that is boolean. -# Instead, this method sets QRegExp instance, that test_message method -# uses. -# -# :type qregexp: QRegExp -# """ -# self._qregexp = qregexp - - def get_regexp(self): - return self._regex - - def set_text(self, text): - """ - Setter for _text - :param text: text to set ''str'' - :emits filter_changed_signal: If _enabled is true - """ - super(TextFilter, self).set_text(text) - - syntax_nr = QRegExp.RegExp - syntax = QRegExp.PatternSyntax(syntax_nr) - self.regex = QRegExp(text, Qt.CaseInsensitive, syntax) - self.set_regex(self.regex) - - def get_text(self): - return self._text diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py deleted file mode 100644 index 8cceb221..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py +++ /dev/null @@ -1,91 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget - - -class TextFilterWidget(QWidget): - """ - Taken from rqt_console.TextFilterWidget. Only modification from it is .ui - file in use that takes more generic form (only textfiedl). - """ - def __init__(self, parentfilter, rospack, display_list_args=None): - """ - Widget for displaying interactive data related to text filtering. - - Taken from rqt_console and simplified to be usable in broader - situations. - - :type parentfilter: BaseFilter - :param parentfilter: buddy filter were data is stored, ''TimeFilter'' - :param display_list_args: empty list, ''list'' - """ - super(TextFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), 'resource', - 'text_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('TextFilterWidget') - # When data is changed it is stored in the parent filter - self._parentfilter = parentfilter - - self.text_edit.textChanged.connect(self.handle_text_changed) - - self.handle_text_changed() - - def set_text(self, text): - """ - Setter for the text edit widget - :param text: text to be placed in text_edit, ''str'' - """ - self.text_edit.setText(text) - - def handle_text_changed(self): - self._parentfilter.set_text(self.text_edit.text()) - - def repopulate(self): - """ - Stub function. - If the widget had any dynamically adjustable data it would requery it - in this function. - """ - pass - - def save_settings(self, settings): - settings.set_value('text', self._parentfilter._text) - - def restore_settings(self, settings): - text = settings.value('text', '') - self.set_text(text) - self.handle_text_changed() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py deleted file mode 100644 index ddacdeff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py +++ /dev/null @@ -1,77 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QStandardItemModel -import rospy - - -class TreenodeItemModel(QStandardItemModel): - """ - This class is made only for this purpose; to hold QStandardItem instances - associated with QModelIndex. QStandardItemModel has methods to return it - by index called itemFromIndex, but in some cases the method doesn't work - for unknown reasons. Ref. question asked: - http://stackoverflow.com/questions/14646979/strange-index-values-from-qstandarditemmodel - - :author: Isaac Saito - """ - - def __init__(self, parent=None): - super(TreenodeItemModel, self).__init__(parent) - self._parent = parent - - self._indexes = {} # { str : QPersistentModelIndex } - - def get_index_from_grn(self, grn): - """ - - :type grn: str - :rtype: QPersistentModelIndex. None if the corresponding item isn't - found. - """ - rospy.logdebug('get_index_from_grn all item={}'.format( - self._indexes)) - return self._indexes.get(grn) - - def set_item_from_index(self, grn, qpindex): - """ - :type grn: str - :type qpindex: QPersistentModelIndex - """ - rospy.logdebug('set_item_from_index grn={} qpindex={}'.format( - grn, qpindex)) - self._indexes[grn] = qpindex diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py deleted file mode 100644 index 8da677f9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py +++ /dev/null @@ -1,265 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -import threading -import time - -import dynamic_reconfigure.client -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QBrush, QStandardItem -import rospy -from rospy.exceptions import ROSException -from rqt_py_common.data_items import ReadonlyItem - -from .dynreconf_client_widget import DynreconfClientWidget - - -class ParamserverConnectThread(threading.Thread): - def __init__(self, parent, param_name_raw): - super(ParamserverConnectThread, self).__init__() - self._parent = parent - self._param_name_raw = param_name_raw - - def run(self): - dynreconf_client = None - try: - dynreconf_client = dynamic_reconfigure.client.Client( - str(self._param_name_raw), timeout=5.0) - rospy.logdebug('ParamserverConnectThread dynreconf_client={}'. \ - format(dynreconf_client)) - self._parent.set_dynreconf_client(dynreconf_client) - except rospy.exceptions.ROSException as e: - raise type(e)(e.message + - "TreenodeQstdItem. Couldn't connect to {}".format( - self._param_name_raw)) - - -class TreenodeQstdItem(ReadonlyItem): - """ - Extending ReadonlyItem - the display content of this item shouldn't be - modified. - """ - - NODE_FULLPATH = 1 - - def __init__(self, *args): - """ - :param args[0]: str (will become 1st arg of QStandardItem) - :param args[1]: integer value that indicates whether this class - is node that has GRN (Graph Resource Names, see - http://www.ros.org/wiki/Names). This can be None - """ - grn_current_treenode = args[0] - self._param_name_raw = grn_current_treenode - self._set_param_name(grn_current_treenode) - super(TreenodeQstdItem, self).__init__(grn_current_treenode) - - # dynamic_reconfigure.client.Client - self._dynreconf_client = None - # DynreconfClientWidget - self._dynreconfclient_widget = None - - self._is_rosnode = False - - self._lock = None - self._paramserver_connect_thread = None - - try: - if args[1]: - self._is_rosnode = True - except IndexError: # tuple index out of range etc. - rospy.logerr('TreenodeQstdItem IndexError') - - def set_dynreconf_client(self, dynreconf_client): - """ - @param dynreconf_client: dynamic_reconfigure.client.Client - """ - self._dynreconf_client = dynreconf_client - rospy.logdebug('Qitem set dynreconf_client={} param={}'.format( - self._dynreconf_client, - self._param_name_raw)) - - def clear_dynreconf_client(self): - if self._dynreconf_client is not None: - self._dynreconf_client.close() - del self._dynreconf_client - self._dynreconf_client = None - - def get_dynreconf_widget(self): - """ - @rtype: DynreconfClientWidget (QWidget) - @return: None if dynreconf_client is not yet generated. - @raise ROSException: - """ - - if not self._dynreconfclient_widget: - rospy.logdebug('get dynreconf_client={}'.format( - self._dynreconf_client)) - rospy.logdebug('In get_dynreconf_widget 1') - if not self._dynreconf_client: - self.connect_param_server() - rospy.logdebug('In get_dynreconf_widget 2') - - timeout = 3 * 100 - loop = 0 - # Loop until _dynreconf_client is set. self._dynreconf_client gets - # set from different thread (in ParamserverConnectThread). - while self._dynreconf_client == None: - #Avoid deadlock - if timeout < loop: - #Make itself unclickable - self.setEnabled(False) - raise ROSException('dynreconf client failed') - - time.sleep(0.01) - loop += 1 - rospy.logdebug('In get_dynreconf_widget loop#{}'.format(loop)) - - rospy.logdebug('In get_dynreconf_widget 4') - self._dynreconfclient_widget = DynreconfClientWidget( - self._dynreconf_client, - self._param_name_raw) - # Creating the DynreconfClientWidget transfers ownership of the _dynreconf_client - # to it. If it is destroyed from Qt, we need to clear our reference to it and - # stop the param server thread we had. - self._dynreconfclient_widget.destroyed.connect(self.clear_dynreconfclient_widget) - self._dynreconfclient_widget.destroyed.connect(self.disconnect_param_server) - rospy.logdebug('In get_dynreconf_widget 5') - - else: - pass - return self._dynreconfclient_widget - - def clear_dynreconfclient_widget(self): - self._dynreconfclient_widget = None - - def connect_param_server(self): - """ - Connect to parameter server using dynamic_reconfigure client. - Behavior is delegated to a private method _connect_param_server, and - its return value, client, is set to member variable. - - @return void - @raise ROSException: - """ - # If the treenode doesn't represent ROS Node, return None. - if not self._is_rosnode: - rospy.logerr('connect_param_server failed due to missing ' + - 'ROS Node. Return with nothing.') - return - - if not self._dynreconf_client: - if self._paramserver_connect_thread: - self.disconnect_param_server() - self._paramserver_connect_thread = ParamserverConnectThread( - self, self._param_name_raw) - self._paramserver_connect_thread.start() - - def disconnect_param_server(self): - if self._paramserver_connect_thread: - # Try to stop the thread - if self._paramserver_connect_thread.isAlive(): - self._paramserver_connect_thread.join(1) - del self._paramserver_connect_thread - self._paramserver_connect_thread = None - self.clear_dynreconf_client() - - def enable_param_items(self): - """ - Create QStdItem per parameter and addColumn them to myself. - :rtype: None if _dynreconf_client is not initiated. - """ - if not self._dynreconfclient_widget: - return None - paramnames = self._dynreconfclient_widget.get_treenode_names() - paramnames_items = [] - brush = QBrush(Qt.lightGray) - for paramname in paramnames: - item = ReadonlyItem(paramname) - item.setBackground(brush) - paramnames_items.append(item) - rospy.logdebug('enable_param_items len of paramnames={}'.format( - len(paramnames_items))) - self.appendColumn(paramnames_items) - - def _set_param_name(self, param_name): - """ - :param param_name: A string formatted as GRN (Graph Resource Names, see - http://www.ros.org/wiki/Names). - Example: /paramname/subpara/subsubpara/... - """ - rospy.logdebug('_set_param_name param_name={} '.format(param_name)) - - # separate param_name by forward slash - self._list_treenode_names = param_name.split('/') - - # Deleting the 1st elem which is zero-length str. - del self._list_treenode_names[0] - - self._toplevel_treenode_name = self._list_treenode_names[0] - - rospy.logdebug('paramname={} nodename={} _list_params[-1]={}'.format( - param_name, self._toplevel_treenode_name, - self._list_treenode_names[-1])) - - def get_param_name_toplv(self): - """ - :rtype: String of the top level param name. - """ - - return self._name_top - - def get_raw_param_name(self): - return self._param_name_raw - - def get_treenode_names(self): - """ - :rtype: List of string. Null if param - """ - - #TODO: what if self._list_treenode_names is empty or null? - return self._list_treenode_names - - def get_node_name(self): - """ - :return: A value of single tree node (ie. NOT the fullpath node name). - Ex. suppose fullpath name is /top/sub/subsub/subsubsub and you - are at 2nd from top, the return value is subsub. - """ - return self._toplevel_treenode_name - - def type(self): - return QStandardItem.UserType diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py deleted file mode 100644 index 675eed16..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py +++ /dev/null @@ -1,89 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - - -from python_qt_binding.QtGui import QModelIndex - - -class TreenodeStatus(QModelIndex): - """ - - This class contains very similar information with - rqt_reconfigure.ParameterItem. The purpose of this class is to enable - FilterChildrenModel (subclassing QSortFilterProxyModel) to look up each - node, which, afaik, is not possible via QSortFilterProxyModel and that's - why I created this class. - - That said, to store an info about each treenode: - - - ParameterItem should be used to show on view. - - This class should be used when you need to keep track from - QAbstractProxyModel - - :author: Isaac Saito - """ - - def __init__(self, nodename_full=None, qmindex=None): - """ - :param index_id: default value is -1, which indicates "not set". This - can be set. - :param nodename_full: default value is None, which indicates "not set". - This can be set. - :type index_id: qint64 - :type nodename_full: str - :type qmindex: QModelIndex - """ - super(TreenodeStatus, self).__init__(qmindex) - - self._is_eval_done = False - self._shows = False - self._nodename_full = nodename_full - - def set_nodename_full(self, nodename_full): - self._nodename_full = nodename_full - - def get_nodename_full(self): - return self._nodename_full - - def set_is_eval_done(self, v): - self._is_eval_done = v - - def get_is_eval_done(self): - return self._is_eval_done - - def set_shows(self, v): - self._shows = v - - def get_shows(self): - return self._shows diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/__init__.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/test/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py deleted file mode 100755 index 0d35524f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py +++ /dev/null @@ -1,67 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from python_qt_binding.QtCore import Qt, QRegExp -from rqt_reconfigure.text_filter import TextFilter - - -class MyTest(unittest.TestCase): - _query_text = 'filter' - _filtered_text = 'filtered_text' - - def setUp(self): - unittest.TestCase.setUp(self) - - syntax_nr = QRegExp.RegExp - syntax = QRegExp.PatternSyntax(syntax_nr) - self._regExp = QRegExp(self._query_text, Qt.CaseInsensitive, syntax) - - self._filter = TextFilter(self._regExp) - self._filter.set_text(self._query_text) - - def tearDown(self): - unittest.TestCase.tearDown(self) - - def test_test_message(self): - """Testing test_message method.""" - result_regex = self._filter.test_message(self._filtered_text) - print 'result_regex={}'.format(result_regex) - self.assertEqual(result_regex, - True # Both _query_text & filtered_text overlaps. - ) - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py deleted file mode 100755 index 3ff17d7f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from rqt_reconfigure.treenode_qstditem import TreenodeQstdItem - - -class TestTreenodeQstdItem(unittest.TestCase): - """ - :author: Isaac Saito - """ - - _nodename_raw = '/base_hokuyo_node' - _nodename_extracted = 'base_hokuyo_node' - - def setUp(self): - unittest.TestCase.setUp(self) - #self._item = TreenodeQstdItem(self._nodename_raw, 0) # For unknown reason - #this stops operation. - self._item = TreenodeQstdItem(self._nodename_raw) - - def tearDown(self): - unittest.TestCase.tearDown(self) - del self._item - - def test_get_node_name(self): - self.assertEqual(self._item.get_node_name(), - self._nodename_extracted) - -# def test_get_node_name(self): -# self.assertEqual(self._item.get_widget().show()) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst deleted file mode 100644 index f309ce56..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst +++ /dev/null @@ -1,104 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_service_caller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt deleted file mode 100644 index 519ee830..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_service_caller) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_service_caller - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox deleted file mode 100644 index 4968e513..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_publisher provides a GUI plugin for publishing arbitrary messages with fixed or computed field values. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/package.xml b/workspace/src/rqt_common_plugins/rqt_service_caller/package.xml deleted file mode 100644 index 98e8b347..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_service_caller - 0.3.13 - rqt_service_caller provides a GUI plugin for calling arbitrary services. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_service_caller - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - rosservice - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml b/workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml deleted file mode 100644 index e29bafa8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin calling ROS services. - - - - - folder - Plugins related to ROS services. - - - media-playback-start - A Python GUI plugin for calling ROS services. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui b/workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui deleted file mode 100644 index 66e56bd0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui +++ /dev/null @@ -1,187 +0,0 @@ - - - ServiceCallerWidget - - - - 0 - 0 - 904 - 359 - - - - Service Caller - - - - - - - - Refresh service list - - - - - - - 16 - 16 - - - - - - - - - 0 - 0 - - - - Service - - - - - - - - 0 - 0 - - - - false - - - - - - - Call selected service - - - Call - - - - 16 - 16 - - - - - - - - - - Qt::Horizontal - - - - - - - Request - - - - - - - Qt::CustomContextMenu - - - Right click on item for more options. - - - true - - - QAbstractItemView::DragOnly - - - QAbstractItemView::ExtendedSelection - - - - Topic - - - - - Type - - - - - Expression - - - - - - - - - - - - Response - - - - - - - Qt::CustomContextMenu - - - Right click on item for more options. - - - true - - - QAbstractItemView::DragOnly - - - QAbstractItemView::ExtendedSelection - - - - Field - - - - - Type - - - - - Value - - - - - - - - - - - - - ExtendedComboBox - QComboBox -
rqt_py_common.extended_combo_box
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller b/workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller deleted file mode 100755 index 49fc7cbc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_service_caller.service_caller.ServiceCaller')) diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/setup.py b/workspace/src/rqt_common_plugins/rqt_service_caller/setup.py deleted file mode 100644 index 7dc34e1d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_service_caller'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/__init__.py b/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py b/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py deleted file mode 100644 index 6719e53b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_gui_py.plugin import Plugin -from .service_caller_widget import ServiceCallerWidget - - -class ServiceCaller(Plugin): - - def __init__(self, context): - super(ServiceCaller, self).__init__(context) - self.setObjectName('ServiceCaller') - - self._widget = ServiceCallerWidget() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - - def trigger_configuration(self): - self._widget.trigger_configuration() diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py b/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py deleted file mode 100644 index 6934470c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py +++ /dev/null @@ -1,284 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import math -import os -import random -import time - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, Slot, qWarning -from python_qt_binding.QtGui import QIcon, QMenu, QTreeWidgetItem, QWidget - -import rospkg -import rospy -import genpy -import rosservice - -from rqt_py_common.extended_combo_box import ExtendedComboBox - - -class ServiceCallerWidget(QWidget): - column_names = ['service', 'type', 'expression'] - - def __init__(self): - super(ServiceCallerWidget, self).__init__() - self.setObjectName('ServiceCallerWidget') - - # create context for the expression eval statement - self._eval_locals = {} - for module in (math, random, time): - self._eval_locals.update(module.__dict__) - self._eval_locals['genpy'] = genpy - del self._eval_locals['__name__'] - del self._eval_locals['__doc__'] - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_service_caller'), 'resource', 'ServiceCaller.ui') - loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox}) - self.refresh_services_button.setIcon(QIcon.fromTheme('view-refresh')) - self.call_service_button.setIcon(QIcon.fromTheme('call-start')) - - self._column_index = {} - for column_name in self.column_names: - self._column_index[column_name] = len(self._column_index) - - self._service_info = None - self.on_refresh_services_button_clicked() - - self.request_tree_widget.itemChanged.connect(self.request_tree_widget_itemChanged) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('splitter_orientation', self.splitter.orientation()) - - def restore_settings(self, plugin_settings, instance_settings): - if int(instance_settings.value('splitter_orientation', Qt.Vertical)) == int(Qt.Vertical): - self.splitter.setOrientation(Qt.Vertical) - else: - self.splitter.setOrientation(Qt.Horizontal) - - def trigger_configuration(self): - new_orientation = Qt.Vertical if self.splitter.orientation() == Qt.Horizontal else Qt.Horizontal - self.splitter.setOrientation(new_orientation) - - @Slot() - def on_refresh_services_button_clicked(self): - service_names = rosservice.get_service_list() - self._services = {} - for service_name in service_names: - try: - self._services[service_name] = rosservice.get_service_class_by_name(service_name) - #qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name])) - except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e: - qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e)) - - self.service_combo_box.clear() - self.service_combo_box.addItems(sorted(service_names)) - - @Slot(str) - def on_service_combo_box_currentIndexChanged(self, service_name): - self.request_tree_widget.clear() - self.response_tree_widget.clear() - service_name = str(service_name) - if not service_name: - return - - self._service_info = {} - self._service_info['service_name'] = service_name - self._service_info['service_class'] = self._services[service_name] - self._service_info['service_proxy'] = rospy.ServiceProxy(service_name, self._service_info['service_class']) - self._service_info['expressions'] = {} - self._service_info['counter'] = 0 - - # recursively create widget items for the service request's slots - request_class = self._service_info['service_class']._request_class - top_level_item = self._recursive_create_widget_items(None, service_name, request_class._type, request_class()) - - # add top level item to tree widget - self.request_tree_widget.addTopLevelItem(top_level_item) - - # resize columns - self.request_tree_widget.expandAll() - for i in range(self.request_tree_widget.columnCount()): - self.request_tree_widget.resizeColumnToContents(i) - - def _recursive_create_widget_items(self, parent, topic_name, type_name, message, is_editable=True): - item = QTreeWidgetItem(parent) - if is_editable: - item.setFlags(item.flags() | Qt.ItemIsEditable) - else: - item.setFlags(item.flags() & (~Qt.ItemIsEditable)) - - if parent is None: - # show full topic name with preceding namespace on toplevel item - topic_text = topic_name - else: - topic_text = topic_name.split('/')[-1] - - item.setText(self._column_index['service'], topic_text) - item.setText(self._column_index['type'], type_name) - - item.setData(0, Qt.UserRole, topic_name) - - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name, type_name in zip(message.__slots__, message._slot_types): - self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name), is_editable) - - elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'): - type_name = type_name.split('[', 1)[0] - for index, slot in enumerate(message): - self._recursive_create_widget_items(item, topic_name + '[%d]' % index, type_name, slot, is_editable) - - else: - item.setText(self._column_index['expression'], repr(message)) - - return item - - @Slot('QTreeWidgetItem*', int) - def request_tree_widget_itemChanged(self, item, column): - column_name = self.column_names[column] - new_value = str(item.text(column)) - #qDebug('ServiceCaller.request_tree_widget_itemChanged(): %s : %s' % (column_name, new_value)) - - if column_name == 'expression': - topic_name = str(item.data(0, Qt.UserRole)) - self._service_info['expressions'][topic_name] = new_value - #qDebug('ServiceCaller.request_tree_widget_itemChanged(): %s expression: %s' % (topic_name, new_value)) - - def fill_message_slots(self, message, topic_name, expressions, counter): - if not hasattr(message, '__slots__'): - return - for slot_name in message.__slots__: - slot_key = topic_name + '/' + slot_name - - # if no expression exists for this slot_key, continue with it's child slots - if slot_key not in expressions: - self.fill_message_slots(getattr(message, slot_name), slot_key, expressions, counter) - continue - - expression = expressions[slot_key] - if len(expression) == 0: - continue - - # get slot type - slot = getattr(message, slot_name) - if hasattr(slot, '_type'): - slot_type = slot._type - else: - slot_type = type(slot) - - self._eval_locals['i'] = counter - value = self._evaluate_expression(expression, slot_type) - if value is not None: - setattr(message, slot_name, value) - - def _evaluate_expression(self, expression, slot_type): - successful_eval = True - successful_conversion = True - - try: - # try to evaluate expression - value = eval(expression, {}, self._eval_locals) - except Exception: - # just use expression-string as value - value = expression - successful_eval = False - - try: - # try to convert value to right type - value = slot_type(value) - except Exception: - successful_conversion = False - - if successful_conversion: - return value - elif successful_eval: - qWarning('ServiceCaller.fill_message_slots(): can not convert expression to slot type: %s -> %s' % (type(value), slot_type)) - else: - qWarning('ServiceCaller.fill_message_slots(): failed to evaluate expression: %s' % (expression)) - - return None - - @Slot() - def on_call_service_button_clicked(self): - self.response_tree_widget.clear() - - request = self._service_info['service_class']._request_class() - self.fill_message_slots(request, self._service_info['service_name'], self._service_info['expressions'], self._service_info['counter']) - try: - response = self._service_info['service_proxy'](request) - except rospy.ServiceException as e: - qWarning('ServiceCaller.on_call_service_button_clicked(): request:\n%r' % (request)) - qWarning('ServiceCaller.on_call_service_button_clicked(): error calling service "%s":\n%s' % (self._service_info['service_name'], e)) - top_level_item = QTreeWidgetItem() - top_level_item.setText(self._column_index['service'], 'ERROR') - top_level_item.setText(self._column_index['type'], 'rospy.ServiceException') - top_level_item.setText(self._column_index['expression'], str(e)) - else: - #qDebug('ServiceCaller.on_call_service_button_clicked(): response: %r' % (response)) - top_level_item = self._recursive_create_widget_items(None, '/', response._type, response, is_editable=False) - - self.response_tree_widget.addTopLevelItem(top_level_item) - # resize columns - self.response_tree_widget.expandAll() - for i in range(self.response_tree_widget.columnCount()): - self.response_tree_widget.resizeColumnToContents(i) - - @Slot('QPoint') - def on_request_tree_widget_customContextMenuRequested(self, pos): - self._show_context_menu(self.request_tree_widget.itemAt(pos), self.request_tree_widget.mapToGlobal(pos)) - - @Slot('QPoint') - def on_response_tree_widget_customContextMenuRequested(self, pos): - self._show_context_menu(self.response_tree_widget.itemAt(pos), self.response_tree_widget.mapToGlobal(pos)) - - def _show_context_menu(self, item, global_pos): - if item is None: - return - - # show context menu - menu = QMenu(self) - action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), "Expand All Children") - action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), "Collapse All Children") - action = menu.exec_(global_pos) - - # evaluate user action - if action in (action_item_expand, action_item_collapse): - expanded = (action is action_item_expand) - - def recursive_set_expanded(item): - item.setExpanded(expanded) - for index in range(item.childCount()): - recursive_set_expanded(item.child(index)) - recursive_set_expanded(item) - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst deleted file mode 100644 index c39eb8ec..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_shell -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt deleted file mode 100644 index 456cf787..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_shell) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_shell - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox deleted file mode 100644 index bb2d90a0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_shell is a Python GUI plugin providing an interactive shell. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_shell/package.xml b/workspace/src/rqt_common_plugins/rqt_shell/package.xml deleted file mode 100644 index a35c84ff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_shell - 0.3.13 - rqt_shell is a Python GUI plugin providing an interactive shell. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_shell - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - qt_gui - qt_gui_py_common - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/plugin.xml b/workspace/src/rqt_common_plugins/rqt_shell/plugin.xml deleted file mode 100644 index e52722fd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin providing an interactive shell. - - - - - folder - Plugins related to miscellaneous tools. - - - terminal - A Python GUI plugin providing an interactive shell. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui b/workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui deleted file mode 100644 index 02403179..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui +++ /dev/null @@ -1,35 +0,0 @@ - - - Shell - - - - 0 - 0 - 276 - 212 - - - - Shell - - - - - - - - - - - - - - ShellTextEdit - QTextEdit -
rqt_shell.shell_text_edit
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell b/workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell deleted file mode 100755 index 57d86771..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_shell.shell.Shell')) diff --git a/workspace/src/rqt_common_plugins/rqt_shell/setup.py b/workspace/src/rqt_common_plugins/rqt_shell/setup.py deleted file mode 100644 index 86521efb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_shell'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/__init__.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py deleted file mode 100644 index 655d34e0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py +++ /dev/null @@ -1,124 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog - -from shell_widget import ShellWidget - -try: - from xterm_widget import XTermWidget, is_xterm_available - _has_xterm = is_xterm_available() -except ImportError: - XTermWidget = None - _has_xterm = False - -try: - from spyder_shell_widget import SpyderShellWidget - _has_spyderlib = True -except ImportError: - SpyderShellWidget = None - _has_spyderlib = False - - -class Shell(Plugin): - """ - Plugin providing an interactive shell - """ - # shell types in order of priority - shell_types = [ - { - 'title': 'XTerm', - 'widget_class': XTermWidget, - 'description': 'Fully functional embedded XTerm (needs xterm and only works on X11).', - 'enabled': _has_xterm, - }, - { - 'title': 'SpyderShell', - 'widget_class': SpyderShellWidget, - 'description': 'Advanced shell (needs spyderlib).', - 'enabled': _has_spyderlib, - }, - { - 'title': 'SimpleShell', - 'widget_class': ShellWidget, - 'description': 'Simple shell for executing non-interactive finite commands.', - 'enabled': True, - }, - ] - - def __init__(self, context): - super(Shell, self).__init__(context) - self._context = context - self.setObjectName('Shell') - - self._widget = None - - def _switch_shell_widget(self): - # check for available shell type - while not self.shell_types[self._shell_type_index]['enabled']: - self._shell_type_index += 1 - selected_shell = self.shell_types[self._shell_type_index] - - if self._widget is not None: - if hasattr(self._widget, 'close_signal'): - self._widget.close_signal.disconnect(self._context.close_plugin) - self._context.remove_widget(self._widget) - self._widget.close() - - self._widget = selected_shell['widget_class']() - self._widget.setWindowTitle(selected_shell['title']) - if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - self._context.add_widget(self._widget) - if hasattr(self._widget, 'close_signal'): - self._widget.close_signal.connect(self._context.close_plugin) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('shell_type', self._shell_type_index) - - def restore_settings(self, plugin_settings, instance_settings): - self._shell_type_index = int(instance_settings.value('shell_type', 0)) - self._switch_shell_widget() - - def trigger_configuration(self): - dialog = SimpleSettingsDialog(title='Shell Options') - dialog.add_exclusive_option_group(title='Shell Type', options=self.shell_types, selected_index=self._shell_type_index) - shell_type = dialog.get_settings()[0] - if shell_type is not None and self._shell_type_index != shell_type['selected_index']: - self._shell_type_index = shell_type['selected_index'] - self._context.reload_plugin() - - def shutdown_plugin(self): - if self._widget is not None and hasattr(self._widget, 'shutdown'): - self._widget.shutdown() - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py deleted file mode 100644 index 5671d6ee..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py +++ /dev/null @@ -1,51 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import subprocess - -from qt_gui_py_common.console_text_edit import ConsoleTextEdit - - -class ShellTextEdit(ConsoleTextEdit): - - def __init__(self, parent=None): - super(ShellTextEdit, self).__init__(parent) - self._add_prompt() - - def _exec_code(self, code): - try: - self._pipe = subprocess.Popen([code], shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) - out, err = self._pipe.communicate() - self._stdout.write(out) - self._stderr.write(err) - except Exception, e: - self._stderr.write(str(e) + '\n') diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py deleted file mode 100644 index 49177360..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py +++ /dev/null @@ -1,47 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -import shell_text_edit - - -class ShellWidget(QWidget): - def __init__(self, parent=None): - super(ShellWidget, self).__init__(parent=parent) - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_shell'), 'resource', 'shell_widget.ui') - loadUi(ui_file, self, {'ShellTextEdit': shell_text_edit.ShellTextEdit}) - self.setObjectName('ShellWidget') diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py deleted file mode 100644 index 2801a40c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py +++ /dev/null @@ -1,121 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QFont, QIcon -from python_qt_binding.QtCore import QProcess, SIGNAL, QTextCodec, Signal - -from spyderlib.widgets.externalshell.baseshell import ExternalShellBase -from spyderlib.widgets.shell import TerminalWidget - - -class SpyderShellWidget(ExternalShellBase): - """Spyder Shell Widget: execute a shell in a separate process using spyderlib's ExternalShellBase""" - SHELL_CLASS = TerminalWidget - close_signal = Signal() - - def __init__(self, parent=None): - ExternalShellBase.__init__(self, parent=parent, fname=None, wdir='.', - history_filename='.history', - light_background=True, - menu_actions=None, - show_buttons_inside=False, - show_elapsed_time=False) - - self.setObjectName('SpyderShellWidget') - - # capture tab key - #self.shell._key_tab = self._key_tab - - self.shell.set_pythonshell_font(QFont('Mono')) - - # Additional python path list - self.path = [] - - # For compatibility with the other shells that can live in the external console - self.is_ipython_kernel = False - self.connection_file = None - - self.create_process() - - def get_icon(self): - return QIcon() - - def create_process(self): - self.shell.clear() - - self.process = QProcess(self) - self.process.setProcessChannelMode(QProcess.MergedChannels) - - env = [unicode(key_val_pair) for key_val_pair in self.process.systemEnvironment()] - env.append('TERM=xterm') - env.append('COLORTERM=gnome-terminal') - self.process.setEnvironment(env) - - # Working directory - if self.wdir is not None: - self.process.setWorkingDirectory(self.wdir) - - self.process.readyReadStandardOutput.connect(self.write_output) - self.process.finished.connect(self.finished) - self.process.finished.connect(self.close_signal) - - self.process.start('/bin/bash', ['-i']) - - running = self.process.waitForStarted() - self.set_running_state(running) - if not running: - self.shell.addPlainText("Process failed to start") - else: - self.shell.setFocus() - self.emit(SIGNAL('started()')) - - return self.process - - def shutdown(self): - self.process.kill() - self.process.waitForFinished() - - def _key_tab(self): - self.process.write('\t') - self.process.waitForBytesWritten(-1) - self.write_output() - - def send_to_process(self, text): - if not isinstance(text, basestring): - text = unicode(text) - if not text.endswith('\n'): - text += '\n' - self.process.write(QTextCodec.codecForLocale().fromUnicode(text)) - self.process.waitForBytesWritten(-1) - - def keyboard_interrupt(self): - self.send_ctrl_to_process('c') diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py deleted file mode 100644 index 5c733463..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding.QtCore import QProcess, QTimer, Signal -from python_qt_binding.QtGui import QX11EmbedContainer - - -class XTermWidget(QX11EmbedContainer): - xterm_cmd = '/usr/bin/xterm' - close_signal = Signal() - - def __init__(self, parent=None): - super(XTermWidget, self).__init__(parent) - self.setObjectName('XTermWidget') - self._process = QProcess(self) - self._process.finished.connect(self.close_signal) - # let the widget finish init before embedding xterm - QTimer.singleShot(100, self._embed_xterm) - - def _embed_xterm(self): - args = ['-into', str(self.winId())] - self._process.start(self.xterm_cmd, args) - if self._process.error() == QProcess.FailedToStart: - print "failed to execute '%s'" % self.xterm_cmd - - def shutdown(self): - self._process.kill() - self._process.waitForFinished() - - -def is_xterm_available(): - return os.path.isfile(XTermWidget.xterm_cmd) - -if __name__ == '__main__': - from PyQt4.QtGui import QApplication - app = QApplication([]) - xt = XTermWidget() - app.exec_() diff --git a/workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst deleted file mode 100644 index da098058..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst +++ /dev/null @@ -1,104 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_srv -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Eliminated a duplicated module with rqt_msg. Now depends on a common module (rqt_msg.MessageWidget) - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt deleted file mode 100644 index 9649d6c4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_srv) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_srv - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox deleted file mode 100644 index 7a0f5839..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_srv - - - ---> - - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_srv/package.xml b/workspace/src/rqt_common_plugins/rqt_srv/package.xml deleted file mode 100644 index 847de737..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_srv - 0.3.13 - A Python GUI plugin for introspecting available ROS message types. - Note that the srvs available through this plugin is the ones that are stored - on your machine, not on the ROS core your rqt instance connects to. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_srv - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - rosmsg - rospy - rqt_gui - rqt_gui_py - rqt_msg - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_srv/plugin.xml b/workspace/src/rqt_common_plugins/rqt_srv/plugin.xml deleted file mode 100644 index 3c540290..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for browsing available ROS service types. - - - - - folder - Plugins related to ROS services. - - - edit-copy - A Python GUI plugin for browsing available ROS service types. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv b/workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv deleted file mode 100755 index 80ce74a7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_srv.services.Services')) diff --git a/workspace/src/rqt_common_plugins/rqt_srv/setup.py b/workspace/src/rqt_common_plugins/rqt_srv/setup.py deleted file mode 100644 index e2225d32..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_srv'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/__init__.py b/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py b/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py deleted file mode 100644 index caeb7b31..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rosmsg - -from qt_gui.plugin import Plugin -from rqt_msg.messages_widget import MessagesWidget - - -class Services(Plugin): -#TODO fix the rosmsg.get_service_class function to return a class with slots like rosmsg.get_message_class does so that the recursive functions used to create tree_view elements will work. - def __init__(self, context): - super(Services, self).__init__(context) - self.setObjectName('servicess') - self._widget = MessagesWidget(rosmsg.MODE_SRV) - self._widget.setWindowTitle('Service Type Browser') - self._widget.type_label.setText('Service:') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # v = instance_settings.value(k) - pass diff --git a/workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst deleted file mode 100644 index 53c9233c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst +++ /dev/null @@ -1,61 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_top -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* rqt_top: fix exception catching -* Contributors: Benjamin Chrétien - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* fix information when nodes are restarted, remove dead nodes from memory (`#294 `_) -* fix rqt_top script (`#303 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- -* fix sort order for numerical fields (`#205 `_) - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* fix an error caused by SIGKILLing nodes - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* remove copy of psutil module and implement missing function (`#105 `_) - -0.2.17 (2013-07-06) -------------------- -* Embeds python-psutil in the package in order to be enabled on Ubuntu Precise -* first release of this package into hydro - diff --git a/workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt deleted file mode 100644 index ba292ce1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_top) -# Load catkin and all dependencies required for this package -#find_package(catkin REQUIRED COMPONENTS python-psutil) -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_top - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_top/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_top/mainpage.dox deleted file mode 100644 index ab3ee1cc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b top - - - ---> - - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_top/package.xml b/workspace/src/rqt_common_plugins/rqt_top/package.xml deleted file mode 100644 index e387302c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - rqt_top - 0.3.13 - RQT plugin for monitoring ROS processes. - - Dan Lazewatsky - Dan Lazewatsky - BSD - - http://ros.org/wiki/rqt_top - https://github.com/ros-visualization/rqt_top - https://github.com/ros-visualization/rqt_top/issues - - catkin - - python-psutil - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_top/plugin.xml b/workspace/src/rqt_common_plugins/rqt_top/plugin.xml deleted file mode 100644 index e46960f9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Provides information similar to that provided in UNIX top for running ROS nodes. - - - - - folder - Plugins related to introspection. - - - utilities-system-monitor - A Python GUI plugin for viewing process information about running ROS processes. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top b/workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top deleted file mode 100755 index ff42031b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2013, Oregon State University -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Oregon State University nor the -# names of its contributors may be used to endorse or promote products -# derived from this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY -# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# Author Dan Lazewatsky/lazewatd@engr.orst.edu - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_top.top_plugin.Top')) diff --git a/workspace/src/rqt_common_plugins/rqt_top/setup.py b/workspace/src/rqt_common_plugins/rqt_top/setup.py deleted file mode 100644 index 1887fb73..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_top'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/__init__.py b/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py b/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py deleted file mode 100644 index 3024b89b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py +++ /dev/null @@ -1,120 +0,0 @@ -# Copyright (c) 2013, Oregon State University -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Oregon State University nor the -# names of its contributors may be used to endorse or promote products -# derived from this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY -# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# Author Dan Lazewatsky/lazewatd@engr.orst.edu - -import rosnode -import rospy -import xmlrpclib -import psutil - -ID = '/NODEINFO' - -class NodeInfo(object): - nodes = dict() - - def get_node_info(self, node_name, skip_cache=False): - node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=skip_cache) - try: - code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID) - if node_name in self.nodes: - return self.nodes[node_name] - else: - try: - p = psutil.Process(pid) - self.nodes[node_name] = p - return p - except: - return False - except xmlrpclib.socket.error: - if not skip_cache: - return self.get_node_info(node_name, skip_cache=True) - else: - return False - - def get_all_node_info(self): - infos = [] - self.remove_dead_nodes() - for node_name in rosnode.get_node_names(): - info = self.get_node_info(node_name) - if info is not False: infos.append((node_name, info)) - return infos - - def get_all_node_fields(self, fields): - processes = self.get_all_node_info() - infos = [] - for name, p in processes: - infos.append(self.as_dict(p, fields + ['cmdline', 'get_memory_info'])) - infos[-1]['node_name'] = name - return infos - - def remove_dead_nodes(self): - running_nodes = rosnode.get_node_names() - dead_nodes = [node_name for node_name in self.nodes if node_name not in running_nodes] - for node_name in dead_nodes: - self.nodes.pop(node_name, None) - - def kill_node(self, node_name): - success, fail = rosnode.kill_nodes([node_name]) - return node_name in success - - def as_dict(self, p, attrs=[], ad_value=None): - # copied code from psutil.__init__ from a newer version - excluded_names = set(['send_signal', 'suspend', 'resume', 'terminate', - 'kill', 'wait', 'is_running', 'as_dict', 'parent', - 'get_children', 'nice']) - retdict = dict() - for name in set(attrs or dir(p)): - if name.startswith('_'): - continue - if name.startswith('set_'): - continue - if name in excluded_names: - continue - try: - attr = getattr(p, name) - if callable(attr): - if name == 'get_cpu_percent': - ret = attr(interval=0) - else: - ret = attr() - else: - ret = attr - except psutil.AccessDenied: - ret = ad_value - except NotImplementedError: - # in case of not implemented functionality (may happen - # on old or exotic systems) we want to crash only if - # the user explicitly asked for that particular attr - if attrs: - raise - continue - if name.startswith('get'): - if name[3] == '_': - name = name[4:] - elif name == 'getcwd': - name = 'cwd' - retdict[name] = ret - return retdict diff --git a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py b/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py deleted file mode 100644 index 4cdc898d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py +++ /dev/null @@ -1,192 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2013, Oregon State University -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Oregon State University nor the -# names of its contributors may be used to endorse or promote products -# derived from this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY -# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# Author Dan Lazewatsky/lazewatd@engr.orst.edu - -from __future__ import division - -from qt_gui.plugin import Plugin -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QLabel, QTreeWidget, QTreeWidgetItem, QVBoxLayout, QCheckBox, QWidget, QToolBar, QLineEdit, QPushButton -from python_qt_binding.QtCore import Qt, QTimer - -from rqt_top.node_info import NodeInfo -import re -from threading import RLock -import textwrap - - -class TopWidgetItem(QTreeWidgetItem): - def __init__(self, parent=None): - super(TopWidgetItem, self).__init__(parent) - - def __lt__(self, other): - col = self.treeWidget().sortColumn() - dtype = Top.SORT_TYPE[col] - return dtype(self.text(col)) < dtype(other.text(col)) - - -class Top(Plugin): - - NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'] - OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ] - FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ] - NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ] - SORT_TYPE = [str, str, float, float, float ] - TOOLTIPS = { - 0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))), - 3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20))) - } - - _node_info = NodeInfo() - - name_filter = re.compile('') - - def __init__(self, context): - super(Top, self).__init__(context) - # Give QObjects reasonable names - self.setObjectName('Top') - - # Process standalone plugin command-line arguments - from argparse import ArgumentParser - parser = ArgumentParser() - # Add argument(s) to the parser. - parser.add_argument("-q", "--quiet", action="store_true", - dest="quiet", - help="Put plugin in silent mode") - args, unknowns = parser.parse_known_args(context.argv()) - # if not args.quiet: - # print 'arguments: ', args - # print 'unknowns: ', unknowns - - self._selected_node = '' - self._selected_node_lock = RLock() - - # Setup the toolbar - self._toolbar = QToolBar() - self._filter_box = QLineEdit() - self._regex_box = QCheckBox() - self._regex_box.setText('regex') - self._toolbar.addWidget(QLabel('Filter')) - self._toolbar.addWidget(self._filter_box) - self._toolbar.addWidget(self._regex_box) - - self._filter_box.returnPressed.connect(self.update_filter) - self._regex_box.stateChanged.connect(self.update_filter) - - # Create a container widget and give it a layout - self._container = QWidget() - self._container.setWindowTitle('Process Monitor') - self._layout = QVBoxLayout() - self._container.setLayout(self._layout) - - self._layout.addWidget(self._toolbar) - - # Create the table widget - self._table_widget = QTreeWidget() - self._table_widget.setObjectName('TopTable') - self._table_widget.setColumnCount(len(self.NODE_LABELS)) - self._table_widget.setHeaderLabels(self.NODE_LABELS) - self._table_widget.itemClicked.connect(self._tableItemClicked) - self._table_widget.setSortingEnabled(True) - self._table_widget.setAlternatingRowColors(True) - - self._layout.addWidget(self._table_widget) - context.add_widget(self._container) - - # Add a button for killing nodes - self._kill_button = QPushButton('Kill Node') - self._layout.addWidget(self._kill_button) - self._kill_button.clicked.connect(self._kill_node) - - # Update twice since the first cpu% lookup will always return 0 - self.update_table() - self.update_table() - - self._table_widget.resizeColumnToContents(0) - - # Start a timer to trigger updates - self._update_timer = QTimer() - self._update_timer.setInterval(1000) - self._update_timer.timeout.connect(self.update_table) - self._update_timer.start() - - def _tableItemClicked(self, item, column): - with self._selected_node_lock: - self._selected_node = item.text(0) - - def update_filter(self, *args): - if self._regex_box.isChecked(): - expr = self._filter_box.text() - else: - expr = re.escape(self._filter_box.text()) - self.name_filter = re.compile(expr) - self.update_table() - - def _kill_node(self): - self._node_info.kill_node(self._selected_node) - - def update_one_item(self, row, info): - twi = TopWidgetItem() - for col, field in enumerate(self.OUT_FIELDS): - val = info[field] - twi.setText(col, self.FORMAT_STRS[col] % val) - self._table_widget.insertTopLevelItem(row, twi) - - for col, (key, func) in self.TOOLTIPS.iteritems(): - twi.setToolTip(col, func(info[key])) - - with self._selected_node_lock: - if twi.text(0) == self._selected_node: - twi.setSelected(True) - - self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0) - - def update_table(self): - self._table_widget.clear() - infos = self._node_info.get_all_node_fields(self.NODE_FIELDS) - for nx, info in enumerate(infos): - self.update_one_item(nx, info) - - def shutdown_plugin(self): - self._update_timer.stop() - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('filter_text', self._filter_box.text()) - instance_settings.set_value('is_regex', int(self._regex_box.checkState())) - - def restore_settings(self, plugin_settings, instance_settings): - self._filter_box.setText(instance_settings.value('filter_text')) - is_regex_int = instance_settings.value('is_regex') - if is_regex_int: - self._regex_box.setCheckState(Qt.CheckState(is_regex_int)) - else: - self._regex_box.setCheckState(Qt.CheckState(0)) - self.update_filter() - - #def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure it - # Usually used to open a configuration dialog diff --git a/workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst deleted file mode 100644 index 79bd94d5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst +++ /dev/null @@ -1,113 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_topic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* check for divide by zero and data failures -* Contributors: Aaron Blasdel - -0.3.12 (2015-07-24) -------------------- -* Save/Restore of headers added -* Contributors: Aaron Blasdel - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* catch and show exceptions `#198 `_ - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* improve rqt_topic initialization time (`#62 `_) -* modified toggling topics to use checkbox instead of context menu (`#75 `_) - -0.3.0 (2013-08-28) ------------------- -* fix cleaning old data in rqt_topic (fix `#74 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- -* Improve API (now either name or msg type are select-able in order to select which topics to monitor). -* API change to accept a list of the topics that this plugin watches. - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt deleted file mode 100644 index a57e51f5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_topic) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_topic - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_topic/package.xml b/workspace/src/rqt_common_plugins/rqt_topic/package.xml deleted file mode 100644 index 6c64d57b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_topic - 0.3.13 - rqt_topic provides a GUI plugin for displaying debug information about ROS topics including publishers, subscribers, publishing rate, and ROS Messages. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_topic - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - rostopic - rqt_gui - rqt_gui_py - std_msgs - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/plugin.xml b/workspace/src/rqt_common_plugins/rqt_topic/plugin.xml deleted file mode 100644 index 901c99f2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin to introspect ROS topics. - - - - - folder - Plugins related to ROS topics. - - - system-search - A Python GUI plugin for monitoring ROS topics. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui b/workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui deleted file mode 100644 index 67430126..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui +++ /dev/null @@ -1,65 +0,0 @@ - - - TopicWidget - - - - 0 - 0 - 731 - 412 - - - - Topic Monitor - - - - - - Qt::CustomContextMenu - - - Right click on item for more options. - - - true - - - QAbstractItemView::DragOnly - - - true - - - - Topic - - - - - Type - - - - - Bandwidth - - - - - Hz - - - - - Value - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic b/workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic deleted file mode 100755 index 34fe7339..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_topic.topic.Topic')) diff --git a/workspace/src/rqt_common_plugins/rqt_topic/setup.py b/workspace/src/rqt_common_plugins/rqt_topic/setup.py deleted file mode 100644 index 44482c44..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_topic'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/__init__.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py deleted file mode 100644 index 721bb9d6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_gui_py.plugin import Plugin - -from .topic_widget import TopicWidget - - -class Topic(Plugin): - - def __init__(self, context): - super(Topic, self).__init__(context) - self.setObjectName('Topic') - - self._widget = TopicWidget(self) - - self._widget.start() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.shutdown_plugin() - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py deleted file mode 100644 index bc28d17b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py +++ /dev/null @@ -1,130 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division, with_statement -from StringIO import StringIO - -from python_qt_binding.QtCore import qWarning - -import roslib -import rospy -from rostopic import ROSTopicHz - - -class TopicInfo(ROSTopicHz): - - def __init__(self, topic_name, topic_type): - super(TopicInfo, self).__init__(100) - self._topic_name = topic_name - self.error = None - self._subscriber = None - self.monitoring = False - self._reset_data() - self.message_class = None - try: - self.message_class = roslib.message.get_message_class(topic_type) - except Exception as e: - self.message_class = None - qWarning('TopicInfo.__init__(): %s' % (e)) - - if self.message_class is None: - self.error = 'can not get message class for type "%s"' % topic_type - qWarning('TopicInfo.__init__(): topic "%s": %s' % (topic_name, self.error)) - - def _reset_data(self): - self.last_message = None - self.times = [] - self.timestamps = [] - self.sizes = [] - - def toggle_monitoring(self): - if self.monitoring: - self.stop_monitoring() - else: - self.start_monitoring() - - def start_monitoring(self): - if self.message_class is not None: - self.monitoring = True - # FIXME: subscribing to class AnyMsg breaks other subscribers on same node - self._subscriber = rospy.Subscriber(self._topic_name, self.message_class, self.message_callback) - - def stop_monitoring(self): - self.monitoring = False - self._reset_data() - if self._subscriber is not None: - self._subscriber.unregister() - - def message_callback(self, message): - ROSTopicHz.callback_hz(self, message) - with self.lock: - self.timestamps.append(rospy.get_time()) - - # FIXME: this only works for message of class AnyMsg - #self.sizes.append(len(message._buff)) - # time consuming workaround... - buff = StringIO() - message.serialize(buff) - self.sizes.append(buff.len) - - if len(self.timestamps) > self.window_size - 1: - self.timestamps.pop(0) - self.sizes.pop(0) - assert(len(self.timestamps) == len(self.sizes)) - - self.last_message = message - - def get_bw(self): - if len(self.timestamps) < 2: - return None, None, None, None - current_time = rospy.get_time() - if current_time <= self.timestamps[0]: - return None, None, None, None - - with self.lock: - total = sum(self.sizes) - bytes_per_s = total / (current_time - self.timestamps[0]) - mean_size = total / len(self.timestamps) - max_size = max(self.sizes) - min_size = min(self.sizes) - return bytes_per_s, mean_size, min_size, max_size - - def get_hz(self): - if not self.times: - return None, None, None, None - with self.lock: - n = len(self.times) - mean = sum(self.times) / n - rate = 1. / mean if mean > 0. else 0 - min_delta = min(self.times) - max_delta = max(self.times) - return rate, mean, min_delta, max_delta diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py deleted file mode 100644 index 8055132f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py +++ /dev/null @@ -1,374 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, QTimer, Signal, Slot -from python_qt_binding.QtGui import QHeaderView, QIcon, QMenu, QTreeWidgetItem, QWidget -import roslib -import rospkg -import rospy -from rospy.exceptions import ROSException - -from .topic_info import TopicInfo - - -class TopicWidget(QWidget): - """ - main class inherits from the ui window class. - - You can specify the topics that the topic pane. - - TopicWidget.start must be called in order to update topic pane. - """ - - SELECT_BY_NAME = 0 - SELECT_BY_MSGTYPE = 1 - - _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value'] - - def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME): - """ - @type selected_topics: list of tuples. - @param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...] - @type select_topic_type: int - @param select_topic_type: Can specify either the name of topics or by - the type of topic, to filter the topics to - show. If 'select_topic_type' argument is - None, this arg shouldn't be meaningful. - """ - super(TopicWidget, self).__init__() - - self._select_topic_type = select_topic_type - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource', 'TopicWidget.ui') - loadUi(ui_file, self) - self._plugin = plugin - self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder) - header = self.topics_tree_widget.header() - header.setResizeMode(QHeaderView.ResizeToContents) - header.customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested) - header.setContextMenuPolicy(Qt.CustomContextMenu) - - # Whether to get all topics or only the topics that are set in advance. - # Can be also set by the setter method "set_selected_topics". - self._selected_topics = selected_topics - - self._current_topic_list = [] - self._topics = {} - self._tree_items = {} - self._column_index = {} - for column_name in self._column_names: - self._column_index[column_name] = len(self._column_index) - - # self.refresh_topics() - - # init and start update timer - self._timer_refresh_topics = QTimer(self) - self._timer_refresh_topics.timeout.connect(self.refresh_topics) - - def set_topic_specifier(self, specifier): - self._select_topic_type = specifier - - def start(self): - """ - This method needs to be called to start updating topic pane. - """ - self._timer_refresh_topics.start(1000) - - @Slot() - def refresh_topics(self): - """ - refresh tree view items - """ - - if self._selected_topics is None: - topic_list = rospy.get_published_topics() - if topic_list is None: - rospy.logerr('Not even a single published topic found. Check network configuration') - return - else: # Topics to show are specified. - topic_list = self._selected_topics - topic_specifiers_server_all = None - topic_specifiers_required = None - - rospy.logdebug('refresh_topics) self._selected_topics=%s' % (topic_list,)) - - if self._select_topic_type == self.SELECT_BY_NAME: - topic_specifiers_server_all = [name for name, type in rospy.get_published_topics()] - topic_specifiers_required = [name for name, type in topic_list] - elif self._select_topic_type == self.SELECT_BY_MSGTYPE: - # The topics that are required (by whoever uses this class). - topic_specifiers_required = [type for name, type in topic_list] - - # The required topics that match with published topics. - topics_match = [(name, type) for name, type in rospy.get_published_topics() if type in topic_specifiers_required] - topic_list = topics_match - rospy.logdebug('selected & published topic types=%s' % (topic_list,)) - - rospy.logdebug('server_all=%s\nrequired=%s\ntlist=%s' % (topic_specifiers_server_all, topic_specifiers_required, topic_list)) - if len(topic_list) == 0: - rospy.logerr('None of the following required topics are found.\n(NAME, TYPE): %s' % (self._selected_topics,)) - return - - if self._current_topic_list != topic_list: - self._current_topic_list = topic_list - - # start new topic dict - new_topics = {} - - for topic_name, topic_type in topic_list: - # if topic is new or has changed its type - if topic_name not in self._topics or \ - self._topics[topic_name]['type'] != topic_type: - # create new TopicInfo - topic_info = TopicInfo(topic_name, topic_type) - message_instance = None - if topic_info.message_class is not None: - message_instance = topic_info.message_class() - # add it to the dict and tree view - topic_item = self._recursive_create_widget_items(self.topics_tree_widget, topic_name, topic_type, message_instance) - new_topics[topic_name] = { - 'item': topic_item, - 'info': topic_info, - 'type': topic_type, - } - else: - # if topic has been seen before, copy it to new dict and - # remove it from the old one - new_topics[topic_name] = self._topics[topic_name] - del self._topics[topic_name] - - # clean up old topics - for topic_name in self._topics.keys(): - self._topics[topic_name]['info'].stop_monitoring() - index = self.topics_tree_widget.indexOfTopLevelItem( - self._topics[topic_name]['item']) - self.topics_tree_widget.takeTopLevelItem(index) - del self._topics[topic_name] - - # switch to new topic dict - self._topics = new_topics - - self._update_topics_data() - - def _update_topics_data(self): - for topic in self._topics.values(): - topic_info = topic['info'] - if topic_info.monitoring: - # update rate - rate, _, _, _ = topic_info.get_hz() - rate_text = '%1.2f' % rate if rate != None else 'unknown' - - # update bandwidth - bytes_per_s, _, _, _ = topic_info.get_bw() - if bytes_per_s is None: - bandwidth_text = 'unknown' - elif bytes_per_s < 1000: - bandwidth_text = '%.2fB/s' % bytes_per_s - elif bytes_per_s < 1000000: - bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.) - else: - bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.) - - # update values - value_text = '' - self.update_value(topic_info._topic_name, topic_info.last_message) - - else: - rate_text = '' - bandwidth_text = '' - value_text = 'not monitored' if topic_info.error is None else topic_info.error - - self._tree_items[topic_info._topic_name].setText(self._column_index['rate'], rate_text) - self._tree_items[topic_info._topic_name].setText(self._column_index['bandwidth'], bandwidth_text) - self._tree_items[topic_info._topic_name].setText(self._column_index['value'], value_text) - - def update_value(self, topic_name, message): - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name in message.__slots__: - self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) - - elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'): - - for index, slot in enumerate(message): - if topic_name + '[%d]' % index in self._tree_items: - self.update_value(topic_name + '[%d]' % index, slot) - else: - base_type_str, _ = self._extract_array_info(self._tree_items[topic_name].text(self._column_index['type'])) - self._recursive_create_widget_items(self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot) - # remove obsolete children - if len(message) < self._tree_items[topic_name].childCount(): - for i in range(len(message), self._tree_items[topic_name].childCount()): - item_topic_name = topic_name + '[%d]' % i - self._recursive_delete_widget_items(self._tree_items[item_topic_name]) - else: - if topic_name in self._tree_items: - self._tree_items[topic_name].setText(self._column_index['value'], repr(message)) - - def _extract_array_info(self, type_str): - array_size = None - if '[' in type_str and type_str[-1] == ']': - type_str, array_size_str = type_str.split('[', 1) - array_size_str = array_size_str[:-1] - if len(array_size_str) > 0: - array_size = int(array_size_str) - else: - array_size = 0 - - return type_str, array_size - - def _recursive_create_widget_items(self, parent, topic_name, type_name, message): - if parent is self.topics_tree_widget: - # show full topic name with preceding namespace on toplevel item - topic_text = topic_name - item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent) - else: - topic_text = topic_name.split('/')[-1] - if '[' in topic_text: - topic_text = topic_text[topic_text.index('['):] - item = QTreeWidgetItem(parent) - item.setText(self._column_index['topic'], topic_text) - item.setText(self._column_index['type'], type_name) - item.setData(0, Qt.UserRole, topic_name) - self._tree_items[topic_name] = item - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name, type_name in zip(message.__slots__, message._slot_types): - self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name)) - - else: - base_type_str, array_size = self._extract_array_info(type_name) - try: - base_instance = roslib.message.get_message_class(base_type_str)() - except (ValueError, TypeError): - base_instance = None - if array_size is not None and hasattr(base_instance, '__slots__'): - for index in range(array_size): - self._recursive_create_widget_items(item, topic_name + '[%d]' % index, base_type_str, base_instance) - return item - - def _toggle_monitoring(self, topic_name): - item = self._tree_items[topic_name] - if item.checkState(0): - self._topics[topic_name]['info'].start_monitoring() - else: - self._topics[topic_name]['info'].stop_monitoring() - - def _recursive_delete_widget_items(self, item): - def _recursive_remove_items_from_tree(item): - for index in reversed(range(item.childCount())): - _recursive_remove_items_from_tree(item.child(index)) - topic_name = item.data(0, Qt.UserRole) - del self._tree_items[topic_name] - _recursive_remove_items_from_tree(item) - item.parent().removeChild(item) - - @Slot('QPoint') - def handle_header_view_customContextMenuRequested(self, pos): - header = self.topics_tree_widget.header() - - # show context menu - menu = QMenu(self) - action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') - action = menu.exec_(header.mapToGlobal(pos)) - - # evaluate user action - if action is action_toggle_auto_resize: - if header.resizeMode(0) == QHeaderView.ResizeToContents: - header.setResizeMode(QHeaderView.Interactive) - else: - header.setResizeMode(QHeaderView.ResizeToContents) - - @Slot('QPoint') - def on_topics_tree_widget_customContextMenuRequested(self, pos): - item = self.topics_tree_widget.itemAt(pos) - if item is None: - return - - # show context menu - menu = QMenu(self) - action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children') - action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children') - action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos)) - - # evaluate user action - if action in (action_item_expand, action_item_collapse): - expanded = (action is action_item_expand) - - def recursive_set_expanded(item): - item.setExpanded(expanded) - for index in range(item.childCount()): - recursive_set_expanded(item.child(index)) - recursive_set_expanded(item) - - def shutdown_plugin(self): - for topic in self._topics.values(): - topic['info'].stop_monitoring() - self._timer_refresh_topics.stop() - - def set_selected_topics(self, selected_topics): - """ - @param selected_topics: list of tuple. [(topic_name, topic_type)] - @type selected_topics: [] - """ - rospy.logdebug('set_selected_topics topics={}'.format( - len(selected_topics))) - self._selected_topics = selected_topics - - # TODO(Enhancement) Save/Restore tree expansion state - def save_settings(self, plugin_settings, instance_settings): - header_state = self.topics_tree_widget.header().saveState() - instance_settings.set_value('tree_widget_header_state', header_state) - - def restore_settings(self, pluggin_settings, instance_settings): - if instance_settings.contains('tree_widget_header_state'): - header_state = instance_settings.value('tree_widget_header_state') - if not self.topics_tree_widget.header().restoreState(header_state): - rospy.logwarn("rqt_topic: Failed to restore header state.") - -class TreeWidgetItem(QTreeWidgetItem): - - def __init__(self, check_state_changed_callback, topic_name, parent=None): - super(TreeWidgetItem, self).__init__(parent) - self._check_state_changed_callback = check_state_changed_callback - self._topic_name = topic_name - self.setCheckState(0, Qt.Unchecked) - - def setData(self, column, role, value): - if role == Qt.CheckStateRole: - state = self.checkState(column) - super(TreeWidgetItem, self).setData(column, role, value) - if role == Qt.CheckStateRole and state != self.checkState(column): - self._check_state_changed_callback(self._topic_name) diff --git a/workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst deleted file mode 100644 index f0af5027..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst +++ /dev/null @@ -1,104 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_web -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt deleted file mode 100644 index 424592cb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_web) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED COMPONENTS) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_web - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_web/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_web/mainpage.dox deleted file mode 100644 index 51e27395..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_web provides a GUI plugin for displaying and interacting with web tools - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_web/package.xml b/workspace/src/rqt_common_plugins/rqt_web/package.xml deleted file mode 100644 index ac87cba9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_web - 0.3.13 - rqt_web is a simple web content viewer for rqt. Users can show web content in Qt-based window by specifying its URL. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_web - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - python-rospkg - qt_gui - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_web/plugin.xml b/workspace/src/rqt_common_plugins/rqt_web/plugin.xml deleted file mode 100644 index 33b57d9f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin displaying and interacting with web tools. - - - - - folder - Plugins related to miscellaneous tools. - - - applications-internet - A Python GUI plugin for displaying and interacting with web tools. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui b/workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui deleted file mode 100644 index 5181c11f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui +++ /dev/null @@ -1,59 +0,0 @@ - - - Web - - - - 0 - 0 - 637 - 557 - - - - Web - - - - 0 - - - 1 - - - - - - - - - - - 27 - 16777215 - - - - - - - - 16 - 16 - - - - true - - - true - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web b/workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web deleted file mode 100755 index 5b0bccd9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_web.web.Web')) diff --git a/workspace/src/rqt_common_plugins/rqt_web/setup.py b/workspace/src/rqt_common_plugins/rqt_web/setup.py deleted file mode 100644 index f08e4d01..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_web'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/__init__.py b/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py b/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py deleted file mode 100644 index f478a892..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py +++ /dev/null @@ -1,72 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin - -from web_widget import WebWidget - - -class Web(Plugin): - """ - Plugin to interface with webtools via ros_gui - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(Web, self).__init__(context) - self.setObjectName('Web') - - # This method is used to allow user to type a url into the url bar - self._web = WebWidget() - if context.serial_number() > 1: - self._web.setWindowTitle(self._web.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._web) - - # This method is used to specify a static url - # NOTE: this method will hide the url bar - #self._web = WebWidget('http://ros.org') - #context.add_widget(self._web) - - # To change the url at a later time use this function - # self._web.set_url('http://willowgarage.com') - - def shutdown_plugin(self): - pass - - def save_settings(self, plugin_settings, instance_settings): - # NOTE: This line is required to save the url bar autocompleter data between sessions - self._web.save_settings(instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - # NOTE: This line is required to restore the url bar autocompleter data between sessions - self._web.restore_settings(instance_settings) diff --git a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py b/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py deleted file mode 100644 index 73e78b1f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py +++ /dev/null @@ -1,174 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, QUrl -from python_qt_binding.QtGui import QCompleter, QIcon, QWidget -from python_qt_binding.QtWebKit import QWebPage, QWebView - - -class WebWidget(QWidget): - def __init__(self, url=None): - """ - Class to load a webpage in a widget. - - :param url: If url is empty then a navigation bar is shown otherwise the url is loaded and the navigation bar is hidden, ''str'' - """ - super(WebWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_web'), 'resource', 'web_widget.ui') - loadUi(ui_file, self) - self.setObjectName('WebWidget') - - self._loading = False - self._stop_icon = QIcon.fromTheme('process-stop') - self._reload_icon = QIcon.fromTheme('view-refresh') - self._working_icon = QIcon.fromTheme('process-working') - - self._completer_word_list = [''] - self._view = QWebView() - self.verticalLayout.addWidget(self._view) - if url is None: - self.set_url("http://ros.org", True) - else: - self.set_url(url, False) - - self.url_lineedit.returnPressed.connect(self._handle_url_change) - self._view.loadFinished[bool].connect(self._handle_load_finished) - self.reload_button.clicked.connect(self._handle_reload_clicked) - self._view.linkClicked.connect(self._handle_link_clicked) - self._view.urlChanged[QUrl].connect(self._handle_url_changed) - - def set_url(self, url, showinput=False): - """ - Sets the url and begins loading that page - :param url: url to load in the webview, ''str or QUrl'' - :param showinput: if true the input bar will be shown, else hidden, ''bool'' - """ - if url is not None: - self._url = QUrl(url) - self.set_show_url_input(showinput) - self._view.setUrl(self._url) - - def set_show_url_input(self, showinput): - """ - Sets the value of the show_url_input flag and hides/shows the widgets as required - :param showinput: true - show inputbar false - hide , ''bool'' - """ - self._show_url_input = showinput - self.url_lineedit.setVisible(self._show_url_input) - self.reload_button.setVisible(self._show_url_input) - if self._show_url_input: - self._view.page().setLinkDelegationPolicy(QWebPage.DelegateAllLinks) - else: - self._view.page().setLinkDelegationPolicy(QWebPage.DontDelegateLinks) - - def save_settings(self, settings): - settings.set_value('url_completion', self._pack(self._completer_word_list)) - settings.set_value('url_current', self._url.toString()) - - def restore_settings(self, settings): - self._completer_word_list += self._unpack(settings.value('url_completion')) - self._completer_word_list = list(set(self._completer_word_list)) - url = settings.value('url_current') - if url: - self.set_url(url, self._show_url_input) - - def _handle_url_change(self): - self.set_url(self.url_lineedit.text(), True) - - def _handle_link_clicked(self, url): - self.set_url(url, True) - - def _handle_reload_clicked(self): - if self._loading: - self._view.stop() - self._loading = False - self.reload_button.setIcon(self._reload_icon) - else: - self._view.reload() - self._loading = True - self.reload_button.setIcon(self._stop_icon) - - def _handle_url_changed(self, url): - # set text to the current loading item - self.url_lineedit.setText(url.toString()) - self.reload_button.setIcon(self._stop_icon) - self._loading = True - - def _handle_load_finished(self, ok): - self._loading = False - self.reload_button.setIcon(self._reload_icon) - if ok: - self._add_completer_list_item(self._url.toString()) - else: - # need to disconnect or we will resend the signal once the error page loads - self._view.loadFinished[bool].disconnect(self._handle_load_finished) - self._view.page().currentFrame().setHtml('

The url you entered seems to be faulty.

') - self._view.loadFinished[bool].connect(self._handle_load_finished) - - def _add_completer_list_item(self, url): - self._completer_word_list.append(self.url_lineedit.text()) - self._completer_word_list = list(set(self._completer_word_list)) - self._completer = QCompleter(self._completer_word_list) - self._completer.setCaseSensitivity(Qt.CaseInsensitive) - self._completer.setCompletionMode(QCompleter.PopupCompletion) - self.url_lineedit.setCompleter(self._completer) - - @staticmethod - def _pack(data): - """ - Packs 'data' into a form that can be easily and readably written to an ini file - :param data: A list of strings to be flattened into a string ''list'' - :return: A string suitable for output to ini files ''str'' - """ - if len(data) == 0: - return '' - if len(data) == 1: - return data[0] - return data - - @staticmethod - def _unpack(data): - """ - Unpacks the values read from an ini file - :param data: An entry taken from an ini file ''list or string'' - :return: A list of strings ''list'' - """ - if data is None or data == '': - data = [] - elif isinstance(data, basestring): - data = [data] - return data From 9ea3c20473bd6774ae437ab1dd6911c159fc5a0c Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 16 Nov 2017 17:21:10 -0800 Subject: [PATCH 124/183] new tuning --- eval_sim/eval_data.jl | 82 ++++++++++--------- scripts/startup.sh | 27 ++++-- workspace/src/barc/src/LMPC_node.jl | 2 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 12 +-- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 2 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 6 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 6 +- 7 files changed, 79 insertions(+), 58 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 35bada43..a7826eda 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -218,49 +218,53 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("One step prediction error for s in lap $i") grid("on") + + + figure(4) + + subplot(221) + plot(t,oldSS.oldSS[1:currentIt,1,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("vx in lap $i") + grid("on") + + subplot(222) + plot(t,oldSS.oldSS[1:currentIt,2,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("vy in lap $i") + grid("on") + subplot(223) + plot(t,oldSS.oldSS[1:currentIt,3,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("psiDot in lap $i") + grid("on") + + subplot(224) + plot(t,oldSS.oldSS[1:currentIt,4,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("ePsi in lap $i") + grid("on") + + figure(5) + subplot(221) + plot(t,oldSS.oldSS[1:currentIt,5,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("eY in lap $i") + grid("on") + + subplot(222) plot(t,oldSS.oldSS[1:currentIt,6,i],"-*") - title("s in curren lap (to check repetitions)") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("s in lap $i") + grid("on") + + subplot(223) + velocity= sqrt(oldSS.oldSS[1:currentIt,2,i].^2 + oldSS.oldSS[1:currentIt,1,i].^2) + plot(t,velocity,"-*") + title("Overall velocity in lap $i") grid("on") - # figure(4) - - # subplot(221) - # plot(t,vx_alpha') - # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - # title("Slack variable for vx in lap $i") - # grid("on") - - # subplot(222) - # plot(t,vy_alpha') - # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - # title("Slack variable for vy in lap $i") - # grid("on") - - # subplot(223) - # plot(t,psiDot_alpha') - # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - # title("Slack variable for psiDot in lap $i") - # grid("on") - - # subplot(224) - # plot(t,ePsi_alpha') - # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - # title("Slack variable for ePsi in lap $i") - # grid("on") - - # figure(5) - # subplot(221) - # plot(t,eY_alpha') - # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - # title("Slack variable for eY in lap $i") - # grid("on") - - # subplot(222) - # plot(t,s_alpha') - # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - # title("Slack variable for s in lap $i") - # grid("on") figure(6) diff --git a/scripts/startup.sh b/scripts/startup.sh index f13ab2c3..16d1c109 100755 --- a/scripts/startup.sh +++ b/scripts/startup.sh @@ -1,7 +1,7 @@ #!/bin/bash # local start up script when opening bash session - +# set environment variable for python export WORKON_HOME=$HOME/.virtualenvs export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv @@ -9,11 +9,28 @@ export VIRTUALENVWRAPPER_VIRTUALENV_ARGS='--no-site-packages' source /usr/local/bin/virtualenvwrapper.sh workon barc -source /home/odroid/team_name.sh +# set environment variables for ROS +source $HOME/barc/workspace/devel/setup.bash +export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ -alias nanorst='cd ~/barc/arduino/.arduino_nano328_node; cp ../arduino_nano328_node/arduino_nano328_node.ino src/; ano clean; ano build -m nano328; ano upload -m nano328 -p /dev/ttyUSB0; roscd barc' -alias rebuild_system='source ~/barc/scripts/rebuild_system.sh' +# set environment variables for Amazon Web Server +source $HOME/team_name.sh -export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ +# define commands +# * nanorst - resets the arduino nano from the command line (assuming the device is connected and on port /dev/ttyUSB0 +# * rebuild_system - rebuild all the ROS packages +alias flash_nano='cd ~/barc/arduino/.arduino_nano328_node; cp ../arduino_nano328_node/arduino_nano328_node.ino src/; ano clean; ano build -m nano328; ano upload -m nano328 -p /dev/ttyUSB0; roscd barc' +alias rebuild_system='source ~/barc/scripts/rebuild_system.sh' +alias tmux='tmux -2' +alias reset_wifi_rules='sudo rm /etc/udev/rules.d/70-persistent-net.rules' +alias set_init_ap='sudo cp $HOME/barc/scripts/accesspoint.conf /etc/init/accesspoint.conf' +# set configuration script for vim text editor cp ~/barc/scripts/vimrc ~/.vimrc + + +# added git branch listing (michael) +parse_git_branch() { + git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/ (\1)/' +} +export PS1="\u@\h \[\033[32m\]\w\[\033[33m\]\$(parse_git_branch)\[\033[00m\] $ " diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index d31fc10e..8556a9ed 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -273,7 +273,7 @@ function main() cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target #save the terminal cost for j = 1:buffersize - cost2target[j] = 0.7*(lapStatus.currentIt-j+1) + cost2target[j] = 1.5*(lapStatus.currentIt-j+1) end oldSS.cost2target[:,lapStatus.currentLap-1] = cost2target diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 98014e14..684b7d91 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -536,12 +536,12 @@ type MpcModel_convhull # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) # end - # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) - # for i=1:N-1 # Constraints on u: - # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) - # end + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + end diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index 3c8acff1..ada79b67 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -93,7 +93,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! idx_s2= findmin(DistS2,1)[2] - off = 3 + off = 2 idx_s2 = idx_s2 + off diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index ddc2a857..844e39a5 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,7 +47,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 16 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 18 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) @@ -64,9 +64,9 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_lane = 10 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 10.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = [20.0,1.0,10.0,20.0,50.0,50.0]#20.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index ea36785d..43effd5d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -93,11 +93,11 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M setvalue(mdl.uPrev,uPrev) setvalue(mdl.coeff,coeffCurvature) if lapStatus.currentLap == 1 - setvalue(mdl.z_Ref,z_ref1) + setvalue(mdl.z_Ref,z_ref2) elseif lapStatus.currentLap == 2 - setvalue(mdl.z_Ref,z_ref1) + setvalue(mdl.z_Ref,z_ref2) elseif lapStatus.currentLap == 3 - setvalue(mdl.z_Ref,z_ref1) + setvalue(mdl.z_Ref,z_ref2) end # Solve Problem and return solution From 3b33574e72c903d447b76b8c73d1b64407e38a1c Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 20 Nov 2017 18:14:49 -0800 Subject: [PATCH 125/183] added working obstacle avoidance --- eval_sim/eval_data.jl | 184 ++++++++++- workspace/src/barc/src/LMPC_node.jl | 119 +++++-- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 295 ++++++++++++++++++ .../src/barc_lib/LMPC/coeffConstraintCost.jl | 36 ++- .../src/barc/src/barc_lib/LMPC/functions.jl | 19 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 65 ++++ workspace/src/barc/src/barc_lib/classes.jl | 4 +- .../src/barc/src/barc_lib/obstaclePosition.jl | 24 ++ 8 files changed, 693 insertions(+), 53 deletions(-) create mode 100644 workspace/src/barc/src/barc_lib/obstaclePosition.jl diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index a7826eda..0d3eae15 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -2,6 +2,7 @@ using JLD using PyPlot using PyCall @pyimport matplotlib.animation as animation +@pyimport matplotlib.patches as patch using JLD, ProfileView # pos_info[1] = s # pos_info[2] = eY @@ -117,6 +118,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) input = Data["input"] cost = Data["mpcCost"] costSlack = Data["mpcCostSlack"] + obs_log = Data["obs_log"] #status = Data["status"] Nl = selectedStates.Nl @@ -129,9 +131,14 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) track = create_track(0.4) + + + for i = laps + pred_sol_xy = xyObstacle(oldSS,obs_log,1,i,track) + println("pred sol= ",pred_sol_xy[:,1]) # for index=1:buffersize # if status[index,i] == :UserLimit # flag[1]=index @@ -163,9 +170,27 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) figure(1) plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(pred_sol_xy[1],pred_sol_xy[2],) plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") grid("on") + ellfig = figure(1) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") + plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 90)#angle=angle_deg) + ax[:add_artist](ell1) + + + grid("on") + title("X-Y view of Lap $i") + t = linspace(1,currentIt,currentIt) @@ -1508,4 +1533,161 @@ function smooth(x,n) y[i,:] = mean(x[start:fin,:],1) end return y -end \ No newline at end of file +end + +function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,track::Array{Float64}) + + obs = obs_log[:,:,obstacle,lap] + + + + buffersize = size(obs)[1] + + + + + + OrderXY = 6 + OrderThetaCurv = 4 + + n_poly = 41 + + ds = 1//10 + + s_vec = zeros(OrderXY+1) + + pred_sol_xy = zeros(2,buffersize,1) + + x_track = track[:,1] + + y_track = track[:,2] + + #println("x_track= ",x_track) + # println("nodes= ",size([x_track'; y_track'])) + for i = 1:buffersize + + + + + nodes = [x_track'; y_track'] + n_nodes = size(x_track)[1] + s_start = (obs[i,1] - 1) + s_end = (obs[i,1] + 3) + s_nearest = obs[i,1] + + idx_start = 10*(floor(obs[i,1]) - 1) + idx_end = 10*(floor(obs[i,1]) + 3) + + if idx_start>n_nodes + idx_start=idx_start%n_nodes + idx_end=idx_end%n_nodes + end + + if idx_start<=0 + nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together + # #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end]) + + #idx_start = n_nodes+idx_start + elseif idx_end>=n_nodes # if the end is behind the finish line + nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes]) # then stack the end and beginning of the lap together + #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes]) + else # if we are somewhere in the middle of the track + nodes_XY = nodes[:,idx_start:idx_end] # then just use the nodes from idx_start to end for interpolation + #nodes_Y = nodes[2,idx_start:idx_end] + end + + + nodes_X = vec(nodes_XY[1,:]) + nodes_Y = vec(nodes_XY[2,:]) + + + itp_matrix = zeros(n_poly,OrderXY+1) + + for ind=1:n_poly + for k=0:OrderXY + + itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k + end + end + + itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1) + + for ind=1:n_poly + for k=0:OrderThetaCurv + + itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k + end + end + + # println("size of nodes x= ",size(nodes_X)) + # println("size of itpmatrix= ",size(itp_matrix)) + # println("s start= ",s_start) + # println("s end= ",s_end) + + coeffY = itp_matrix\nodes_Y + coeffX = itp_matrix\nodes_X + + + b_curvature_vector = zeros(n_poly) + + Counter = 1 + + + for ind = 0:n_poly-1 + s_expression_der = zeros(OrderXY+1) + s_expression_2der = zeros(OrderXY+1) + s_poly = s_start + ind*ds + for k=0:OrderXY-1 + s_expression_der[OrderXY-k] = (k+1)*s_poly^k + end + for k=0:OrderXY-2 + s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k + end + + dX = dot(coeffX,s_expression_der) + dY = dot(coeffY,s_expression_der) + ddX = dot(coeffX,s_expression_2der) + ddY = dot(coeffY,s_expression_2der) + + curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula + + b_curvature_vector[Counter] = curvature + + Counter = Counter + 1 + end + + + + coeffCurv = itp_matrix_curv\b_curvature_vector + + s0 = obs[i,1]+0.001 + + s_vec = zeros(OrderXY+1)::Array{Float64} + sdot_vec = zeros(OrderXY+1)::Array{Float64} + + for k = 1:OrderXY+1 + s_vec[k] = obs[i,1]^(OrderXY-k+1) + + end + for k = 1:OrderXY + sdot_vec[k] = (OrderXY+1-k)* obs[i,1]^(OrderXY-k) + end + + + XCurve = dot(coeffX,s_vec) + YCurve = dot(coeffY,s_vec) + + dX = dot(coeffX,sdot_vec) + dY = dot(coeffY,sdot_vec) + + + xyPathAngle = atan2(dY,dX) + + pred_sol_xy[2,i] = YCurve + obs[i,2]*cos(xyPathAngle) + pred_sol_xy[1,i] = XCurve - obs[i,2]*sin(xyPathAngle) + + + end + + return pred_sol_xy +end diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 8556a9ed..e18adecf 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -17,6 +17,7 @@ include("barc_lib/LMPC/MPC_models.jl") include("barc_lib/LMPC/coeffConstraintCost.jl") include("barc_lib/LMPC/solveMpcProblem.jl") include("barc_lib/simModel.jl") +include("barc_lib/obstaclePosition.jl") # This function is called whenever a new state estimate is received. # It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) @@ -95,6 +96,7 @@ function main() mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) #mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + mdl_obstacle = MpcModel_obstacle(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates,obstacle) max_N = max(mpcParams.N,mpcParams_pF.N) @@ -138,6 +140,7 @@ function main() log_status = Array(Symbol,buffersize,30) log_mpcCost = zeros(buffersize,6,30) log_mpcCostSlack = zeros(buffersize,6,30) + log_obs = zeros(buffersize,3,obstacle.n_obs,30) acc_f = [0.0] @@ -190,7 +193,7 @@ function main() posInfo.s = posInfo.s_target/2 lapStatus.currentLap = 4 oldTraj.count[4] = 500 - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus,selectedStates,oldSS) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus,selectedStates,oldSS,obs_curr[1,:,:],obstacle) oldTraj.count[4] = 1 lapStatus.currentLap = 1 oldTraj.oldTraj[1:buffersize,6,1] = NaN*ones(buffersize,1) @@ -214,11 +217,11 @@ function main() same_sPF = 0 same_sLMPC = 0 - # #### Set initial conditions on the obstacles + #### Set initial conditions on the obstacles - # obs_curr[1,1,:] = obstacle.s_obs_init - # obs_curr[1,2,:] = obstacle.ey_obs_init - # obs_curr[1,3,:] = obstacle.v_obs_init + obs_curr[1,1,:] = obstacle.s_obs_init + obs_curr[1,2,:] = obstacle.ey_obs_init + obs_curr[1,3,:] = obstacle.v_obs_init # Start node while ! is_shutdown() @@ -270,7 +273,7 @@ function main() # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj oldSS.oldCost[lapStatus.currentLap-1] = lapStatus.currentIt - cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target + cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target #save the terminal cost for j = 1:buffersize cost2target[j] = 1.5*(lapStatus.currentIt-j+1) @@ -278,12 +281,12 @@ function main() oldSS.cost2target[:,lapStatus.currentLap-1] = cost2target - # if lapStatus.currentLap == obstacle.lap_active # if its time to put the obstacles in the track - # obstacle.obstacle_active = true # tell the system to put the obstacles on the track - # end - # if lapStatus.currentLap > obstacle.lap_active # initialize current obstacle states with final states from the previous lap - # obs_curr[1,:,:] = obs_curr[i,:,:] - # end + if lapStatus.currentLap == obstacle.lap_active # if its time to put the obstacles in the track + obstacle.obstacle_active = true # tell the system to put the obstacles on the track + end + if lapStatus.currentLap > obstacle.lap_active # initialize current obstacle states with final states from the previous lap + obs_curr[1,:,:] = obs_curr[i,:,:] + end zCurr[1,:] = zCurr[i,:] # copy current state @@ -301,23 +304,45 @@ function main() setvalue(mdl.z_Ol[1:mpcParams.N,5],mpcSol.z[1:mpcParams.N,2]) setvalue(mdl.z_Ol[1:mpcParams.N,4],mpcSol.z[1:mpcParams.N,3]) setvalue(mdl.u_Ol,mpcSol.u[1:mpcParams.N,:]) - end + + elseif selectedStates.version == false && obstacle.obstacle_active == false + + setvalue(mdl_convhull.z_Ol[:,1],zCurr[1,1]*ones(mpcParams.N+1))#mpcSol.z[:,4]) + setvalue(mdl_convhull.z_Ol[:,6],zCurr[1,6]*ones(mpcParams.N+1))#mpcSol.z[:,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[:,5],zCurr[1,5]*ones(mpcParams.N+1))#mpcSol.z[:,2]) + setvalue(mdl_convhull.z_Ol[:,4],zCurr[1,4]*ones(mpcParams.N+1))#mpcSol.z[:,3]) + setvalue(mdl_convhull.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) + setvalue(mdl_convhull.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) + #setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) + setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + + elseif selectedStates.version == false && obstacle.obstacle_active == true + + setvalue(mdl_obstacle.z_Ol[:,1],zCurr[1,1]*ones(mpcParams.N+1))#mpcSol.z[:,4]) + setvalue(mdl_obstacle.z_Ol[:,6],zCurr[1,6]*ones(mpcParams.N+1))#mpcSol.z[:,1]-posInfo.s_target) + setvalue(mdl_obstacle.z_Ol[:,5],zCurr[1,5]*ones(mpcParams.N+1))#mpcSol.z[:,2]) + setvalue(mdl_obstacle.z_Ol[:,4],zCurr[1,4]*ones(mpcParams.N+1))#mpcSol.z[:,3]) + setvalue(mdl_obstacle.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) + setvalue(mdl_obstacle.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) + #setvalue(mdl_obstacle.u_Ol,mpcSol.u[1:mpcParams.N,:]) + setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) - setvalue(mdl_convhull.z_Ol[:,1],zCurr[1,1]*ones(mpcParams.N+1))#mpcSol.z[:,4]) - setvalue(mdl_convhull.z_Ol[:,6],zCurr[1,6]*ones(mpcParams.N+1))#mpcSol.z[:,1]-posInfo.s_target) - setvalue(mdl_convhull.z_Ol[:,5],zCurr[1,5]*ones(mpcParams.N+1))#mpcSol.z[:,2]) - setvalue(mdl_convhull.z_Ol[:,4],zCurr[1,4]*ones(mpcParams.N+1))#mpcSol.z[:,3]) - setvalue(mdl_convhull.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) - setvalue(mdl_convhull.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) - #setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) - setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + end elseif lapStatus.currentLap > n_pf+1 if selectedStates.version == true + setvalue(mdl.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - end - setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + elseif selectedStates.version == false && obstacle.obstacle_active == false + + setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + + elseif selectedStates.version == false && obstacle.obstacle_active == true + + setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + end end end @@ -332,7 +357,7 @@ function main() #println("s = $(posInfo.s)") #println("s_total = $(posInfo.s%posInfo.s_target)") - #mpcParams.Q_obs = ones(selectedStates.Nl*selectedStates.Np) + mpcParams.Q_obs = ones(selectedStates.Nl*selectedStates.Np) @@ -343,11 +368,38 @@ function main() end end + ## if obstacles are on the track, find the nearest one + + if obstacle.obstacle_active == true + + obs_temp = obs_curr[lapStatus.currentIt,:,:] + + + + if posInfo.s_target-posInfo.s < obstacle.obs_detect # meaning that I could possibly detect obstacles after the finish line + + index1=find(obs_curr[lapStatus.currentIt,1,:].< obstacle.obs_detect+posInfo.s-posInfo.s_target) # look for obstacles that could cause problems + + obs_temp[1,1,index1] = posInfo.s_target + obs_curr[lapStatus.currentIt,1,index1] + + + end + + + dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) + + # println("dist= ",dist) + # println("obstacle's position= ",obs_temp[1,1,:]) + obs_near = obs_temp[1,:,index] + end + + + # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() - coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus,selectedStates,oldSS) + coeffConstraintCost(oldTraj,mpcCoeff,posInfo,mpcParams,lapStatus,selectedStates,oldSS,obs_curr[lapStatus.currentIt,:,:],obstacle) tt = toq() #println("Finished coefficients, t = ",tt," s") end @@ -366,9 +418,13 @@ function main() zCurr[i,7] = acc0 if selectedStates.version == true solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) - else + elseif selectedStates.version == false && obstacle.obstacle_active == false solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) #solveMpcProblem_test(mdl_test,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) + elseif selectedStates.version == false && obstacle.obstacle_active == true + + solveMpcProblem_obstacle(mdl_obstacle,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates,obs_near,obstacle) + end acc0 = mpcSol.z[2,7] @@ -386,6 +442,10 @@ function main() log_t_solv[k+1] = toq() #println("time= ",log_t_solv[k+1]) + if obstacle.obstacle_active == true + obs_curr[lapStatus.currentIt+1,:,:] = obstaclePosition(obs_curr[i,:,:],modelParams,obstacle,posInfo) + end + # Send command immediately, only if it is optimal! #if mpcSol.solverStatus == :Optimal # opt_count = 0 @@ -421,6 +481,7 @@ function main() log_cpsi[lapStatus.currentIt,:,lapStatus.currentLap] = mpcCoeff.c_Psi log_input[lapStatus.currentIt,:,lapStatus.currentLap] = uCurr[i,:] log_mpcCost[lapStatus.currentIt,:,lapStatus.currentLap] = mpcSol.cost + log_obs[lapStatus.currentIt,:,:,lapStatus.currentLap] = obs_curr[i,:,:] if lapStatus.currentLap > n_pf log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack end @@ -473,7 +534,7 @@ function main() "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k],"selectedStates",selectedStates,"one_step_error",log_onestep, "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"lapStatus",lapStatus, "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy,"input",log_input, - "mpcCost",log_mpcCost,"mpcCostSlack",log_mpcCostSlack) + "mpcCost",log_mpcCost,"mpcCostSlack",log_mpcCostSlack,"obs_log",log_obs) #,"status",log_status) println("Exiting LMPC node. Saved data to $log_path.") @@ -483,9 +544,9 @@ function main() # println("postions= ",oldSS.oldSS[1:20,6,lapStatus.currentLap]) # println("selected States= ",selStates_log[:,6,1:20,lapStatus.currentLap] ) # println("states cost= ",statesCost_log[:,1:20,lapStatus.currentLap]) - end + if ! isinteractive() main() end diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 684b7d91..9ef8d1bb 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -899,3 +899,298 @@ type MpcModel_test end end +type MpcModel_obstacle + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,1} + c_Vy::Array{JuMP.NonlinearParameter,1} + c_Psi::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + Q_obs::Array{JuMP.NonlinearParameter,1} + obs::Array{JuMP.NonlinearParameter,2} + + + eps_lane::Array{JuMP.Variable,1} + #eps_vel::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearExpression,1} + + derivCost::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + slackVx::JuMP.NonlinearExpression + slackVy::JuMP.NonlinearExpression + slackPsidot::JuMP.NonlinearExpression + slackEpsi::JuMP.NonlinearExpression + slackEy::JuMP.NonlinearExpression + slackS::JuMP.NonlinearExpression + obstacleSlackCost::JuMP.NonlinearExpression + + #velocityCost::JuMP.NonlinearExpression + + function MpcModel_obstacle(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates,obstacle::Obstacle) + + + m = new() + + #### Initialize parameters + + dt = modelParams.dt # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + u_lb = modelParams.u_lb # lower bounds for the control inputs + u_ub = modelParams.u_ub # upper bounds for the control inputs + z_lb = modelParams.z_lb # lower bounds for the states + z_ub = modelParams.z_ub # upper bounds for the states + c0 = modelParams.c0 + + + ey_max = trackCoeff.width/2 # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons + n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree for curvature approximation + v_max = 3 # maximum allowed velocity + + v_ref = mpcParams.vPathFollowing # reference velocity for the path following + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + #Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity + delay_df = mpcParams.delay_df + delay_a = mpcParams.delay_a + Q_slack = mpcParams.Q_slack + + r_s = obstacle.r_s + r_ey = obstacle.r_ey + + + Np = selectedStates.Np::Int64 # how many states to select + Nl = selectedStates.Nl::Int64 # how many previous laps to select + + acc_f = 1.0 + + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @variable( mdl, z_Ol[1:(N+1),1:7]) + @variable( mdl, u_Ol[1:N,1:2]) + @variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha + #@variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity + + + + z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states + z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_6s[j,i]) + setupperbound(u_Ol[j,i], u_ub_6s[j,i]) + end + end + for i=1:7 + for j=1:N+1 + setlowerbound(z_Ol[j,i], z_lb_6s[j,i]) + setupperbound(z_Ol[j,i], z_ub_6s[j,i]) + end + end + + @NLparameter(mdl, z0[i=1:7] == 0) + @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0) + @NLparameter(mdl, c_Vx[i=1:3] == 0) + @NLparameter(mdl, c_Vy[i=1:4] == 0) + @NLparameter(mdl, c_Psi[i=1:3] == 0) + @NLparameter(mdl, uPrev[1:10,1:2] == 0) + @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" + @NLparameter(mdl, Q_obs[i=1:Nl*Np] == mpcParams.Q_obs[i]) # weight used to exclude some states from the convex hull + @NLparameter(mdl, obs[j=1:N+1,i=1:3] == 0) # nearest obstacle to avoid + + + # Conditions for first solve: + setvalue(z0[1],1) + setvalue(c_Vx[3],0.1) + + @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i]) + + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) + #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity + @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull + + #for n = 1:6 + #@NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:Nl*Np}) # terminal constraint + #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n]) + #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n]) + #end + + @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) + @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i])) + + println("Initializing model...") + + # System dynamics + for i=1:N + if i<=delay_df + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2]) # psiDot + else + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2]) # psiDot + end + if i<=delay_a + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1])) + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f) + else + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot + @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f) + end + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7]) # xDot + #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1])) # xDot + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7]) + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i])) # ePsi + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY + @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s + end + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end + + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + end + + + + # Cost functions + + # Derivative cost + # --------------------------------- + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} + + QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ + QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) + + # Control Input cost + # --------------------------------- + @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+ + R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df}) + + # Lane cost (soft) + # --------------------------------- + @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) + + # Terminal Cost + # --------------------------------- + @NLexpression(mdl, terminalCost , sum{Q_obs[i]*alpha[i]*statesCost[i], i=1:Nl*Np}) + + # Slack cost (soft) + # --------------------------------- + #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) + + # Slack cost on vx + #---------------------------------- + @NLexpression(mdl, slackVx, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1],j=1:Nl*Np})^2) + + # Slack cost on vy + #---------------------------------- + @NLexpression(mdl, slackVy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2],j=1:Nl*Np})^2) + + # Slack cost on Psi dot + #---------------------------------- + @NLexpression(mdl, slackPsidot, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3],j=1:Nl*Np})^2) + + # Slack cost on ePsi + #---------------------------------- + @NLexpression(mdl, slackEpsi, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4],j=1:Nl*Np})^2) + + # Slack cost on ey + #---------------------------------- + @NLexpression(mdl, slackEy, (z_Ol[N+1,5] - sum{alpha[j]*selStates[j,5],j=1:Nl*Np})^2) + + # Slack cost on s + #---------------------------------- + @NLexpression(mdl, slackS, (z_Ol[N+1,6] - sum{alpha[j]*selStates[j,6],j=1:Nl*Np})^2) + + # Soft Constraint on the Obstacle + # -------------------------------- + @NLexpression(mdl, obstacleSlackCost, 1*sum{-log(((z_Ol[i,6]-obs[i,1])/r_s)^2 + ((z_Ol[i,5]-obs[i,2])/r_ey)^2 -1),i=1:N+1}) + + + # Velocity Cost + #---------------------------------- + #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) + + # Overall Cost function (objective of the minimization) + # ----------------------------------------------------- + + #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + + @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS + obstacleSlackCost) + + + + sol_stat=solve(mdl) + println("Finished solve 1 convhull mpc: $sol_stat") + sol_stat=solve(mdl) + println("Finished solve 2 convhull mpc: $sol_stat") + + + m.mdl = mdl + m.z0 = z0 + m.coeff = coeff + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.uPrev = uPrev + m.Q_obs = Q_obs # weigth to exclude states from optimization problem + m.obs = obs # obstacle to avoid + #m.eps_alpha=eps_alpha + + m.derivCost = derivCost + m.controlCost = controlCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + #m.velocityCost= velocityCost #velocity cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + + m.obstacleSlackCost=obstacleSlackCost + + m.slackVx = slackVx + m.slackVy = slackVy + m.slackPsidot = slackPsidot + m.slackEpsi = slackEpsi + m.slackEy = slackEy + m.slackS = slackS + + + return m + end +end + diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index ada79b67..1c7575ed 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -17,7 +17,7 @@ # z[6] = s function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams,lapStatus::LapStatus, - selectedStates::SelectedStates,oldSS::SafeSetData) + selectedStates::SelectedStates,oldSS::SafeSetData,obs::Array{Float64},obstacle::Obstacle) # this computes the coefficients for the cost and constraints # Outputs: @@ -28,6 +28,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo s = posInfo.s s_target = posInfo.s_target + dt = 0.1 + N = mpcParams.N + Np = selectedStates.Np Nl = selectedStates.Nl @@ -96,6 +99,11 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo off = 2 idx_s2 = idx_s2 + off + # Propagate the obstacle for the prediction horizon + + obs_prop_s = obs[1,1,:] + dt*N*obs[1,3,:] + obs_prop_ey = obs[1,2,:] + ####################################################################### @@ -105,22 +113,24 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]] # and their cost - # if obstacle.lap_active == true # if the obstacles are on the track, check if any of the selected states interferes with the propagated obstacle - - # for n=1:obstacle.n_obs - # ellipse_check = (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,6]-obs_prop_s[n])/r_s)^2) + (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,5]-obs_prop_ey[n])/r_ey)^2) + if obstacle.obstacle_active == true # if the obstacles are on the track, check if any of the selected states interferes with the propagated obstacle + for n=1:obstacle.n_obs + ellipse_check = (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,6]-obs_prop_s[n])/obstacle.r_s).^2) + (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,5]-obs_prop_ey[n])/obstacle.r_ey).^2) - # if any(x->x<=1, ellipse_check) == true # if any of the selected states is in the ellipse - - # index = find(ellipse_check.<=1) # find all the states in the ellipse - - # mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] = 10 # and set the values of the weight to 10, so that they are excluded from optimization - # end - # end - # end + if any(x->x<=1, ellipse_check) == true # if any of the selected states is in the ellipse + + index = find(ellipse_check.<=1) # find all the states in the ellipse + #println("index= ",index[1]) + mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] = 10 # and set the values of the weight to 10, so that they are excluded from optimization + end + end + end end + #println("Q_obs= ",mpcParams.Q_obs) + + ########################################################################## if selectedStates.version == true vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 844e39a5..7fdfccdf 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -65,8 +65,9 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 10 # weight on the soft constraint for the lane - mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity mpcParams.Q_slack = [20.0,1.0,10.0,20.0,50.0,50.0]#20.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories @@ -123,16 +124,16 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentLap = 1 # initialize lap number lapStatus.currentIt = 0 # current iteration in lap - obstacle.obstacle_active = false # true if we have t consider the obstacles in the optimization problem - obstacle.lap_active = 20 # number of the first lap in which the obstacles are used + obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem + obstacle.lap_active = 5 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [20] # initial s coordinate of each obstacle - obstacle.ey_obs_init = [-0.8] # initial ey coordinate of each obstacle - obstacle.v_obs_init = [1.5] # initial velocity of each obstacles - obstacle.r_s = 0.5 - obstacle.r_ey = 0.2 - obstacle.inv_step = 3 # number of step of invariance required for the safe set + obstacle.s_obs_init = [19] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [-0.2] # initial ey coordinate of each obstacle + obstacle.v_obs_init = [0] # initial velocity of each obstacles + obstacle.r_s = 0.2#0.5 + obstacle.r_ey = 0.1#0.2 + obstacle.inv_step = 0 # number of step of invariance required for the safe set end # Use this function to smooth data (moving average) diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index 43effd5d..d1a8455c 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -210,3 +210,68 @@ function solveMpcProblem_test(m::MpcModel_test,mpcSol::MpcSol,mpcCoeff::MpcCoeff nothing end +function solveMpcProblem_obstacle(m::MpcModel_obstacle,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus, + posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},selectedStates::SelectedStates, + obs_now::Array{Float64},obstacle::Obstacle) + + # Load Parameters + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + selStates = selectedStates.selStates::Array{Float64,2} + statesCost = selectedStates.statesCost::Array{Float64,1} + + Q_obs = mpcParams.Q_obs::Array{Float64,1} + +# println("Q_obs= ",Q_obs) + + obs = zeros(mpcParams.N+1,3) + obs[1,:] = obs_now + + # Compute the position of the obstacle in the whole prediction horizon + + for i = 1:mpcParams.N + obs[i+1,1] = obs[i,1] + modelParams.dt*i*obs[i,3] + obs[i+1,2] = obs[i,2] + obs[i+1,3] = obs[i,3] + end + + # println("obstacle= ",obs) + + # Update current initial condition, curvature and System ID coefficients + setvalue(m.z0,zCurr) + setvalue(m.uPrev,uPrev) + setvalue(m.c_Vx,mpcCoeff.c_Vx) # System ID coefficients + setvalue(m.c_Vy,mpcCoeff.c_Vy) + setvalue(m.c_Psi,mpcCoeff.c_Psi) + setvalue(m.coeff,trackCoeff.coeffCurvature) # Track curvature + setvalue(m.selStates,selStates) + setvalue(m.statesCost,statesCost) + setvalue(m.Q_obs,Q_obs) + setvalue(m.obs,obs) + + # Solve Problem and return solution + sol_status = solve(m.mdl) + sol_u = getvalue(m.u_Ol) + sol_z = getvalue(m.z_Ol) + + # export data + mpcSol.a_x = sol_u[1,1] + mpcSol.d_f = sol_u[1,2] + mpcSol.u = sol_u + mpcSol.z = sol_z + #mpcSol.eps_alpha = getvalue(m.eps_alpha) + mpcSol.solverStatus = sol_status + mpcSol.cost = zeros(6) + mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] + + mpcSol.costSlack = zeros(6) + mpcSol.costSlack = [getvalue(m.slackVx),getvalue(m.slackVy),getvalue(m.slackPsidot),getvalue(m.slackEpsi),getvalue(m.slackEy),getvalue(m.slackS)] + + #println("cost for obstacle= ",getvalue(m.obstacleSlackCost)) + + println("Solved, status = $sol_status") + + nothing +end \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index e8ce1f41..02c2cfde 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -79,9 +79,11 @@ type MpcParams # parameters for MPC solver Q_lane::Float64 # weight on the soft constraint for the lane Q_vel::Float64 # weight on the soft constraint for the maximum velocity Q_slack::Array{Float64,1} # weight on the slack variables for the terminal constraint + Q_obs::Array{Float64} # weight used to esclude some of the old trajectories from the optimization problem - MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0,Q_lane=1.0,Q_vel=1.0,Q_slack=Float64[]) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a,Q_lane,Q_vel,Q_slack) + + MpcParams(N=0,nz=0,OrderCostCons=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0,Q_lane=1.0,Q_vel=1.0,Q_slack=Float64[],Q_obs=Float64[]) = new(N,nz,OrderCostCons,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a,Q_lane,Q_vel,Q_slack,Q_obs) end type PosInfo # current position information diff --git a/workspace/src/barc/src/barc_lib/obstaclePosition.jl b/workspace/src/barc/src/barc_lib/obstaclePosition.jl new file mode 100644 index 00000000..c86a09ad --- /dev/null +++ b/workspace/src/barc/src/barc_lib/obstaclePosition.jl @@ -0,0 +1,24 @@ +# This file contains all the functions related to the obstacles + +# this function computes the position of the obstacle +function obstaclePosition(obs_now::Array{Float64},modelParams::ModelParams,obstacle::Obstacle,posInfo::PosInfo) + + n_obs = obstacle.n_obs + obs_next=zeros(1,3,n_obs)::Array{Float64} # initialize the array that will contain the next states of the obstacle + + dt = modelParams.dt + + obs_next[:,1,:] = (obs_now[:,1,:] + obs_now[:,3,:]*dt)%posInfo.s_target # s + obs_next[:,2,:] = obs_now[:,2,:] # ey + obs_next[:,3,:] = obs_now[:,3,:] # v + + # if any(x->x>posInfo.s_target, obs_next[1,1,:]) == true + + # index = find(obs_next[1,1,:].>posInfo.s_target) + + # obs_next[1,1,index] = obs_next[1,1,index] - posInfo.s_target + # end + + return obs_next + +end \ No newline at end of file From 6df6d65fab6039a901539e3eb76a5486194aa6c7 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 21 Nov 2017 16:05:05 -0800 Subject: [PATCH 126/183] minor changes to plotting --- eval_sim/eval_data.jl | 30 +++++++++++-------- .../src/barc/src/barc_lib/LMPC/functions.jl | 4 +-- 2 files changed, 19 insertions(+), 15 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 0d3eae15..d9811de7 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -184,7 +184,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 90)#angle=angle_deg) + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 0)#angle=angle_deg) ax[:add_artist](ell1) @@ -1545,14 +1545,14 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra + # println("obs= ",obs) + OrderXY = 18 + OrderThetaCurv = 8 - OrderXY = 6 - OrderThetaCurv = 4 - - n_poly = 41 + - ds = 1//10 + ds = 0.0625 s_vec = zeros(OrderXY+1) @@ -1572,16 +1572,19 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra nodes = [x_track'; y_track'] n_nodes = size(x_track)[1] s_start = (obs[i,1] - 1) - s_end = (obs[i,1] + 3) + s_end = (obs[i,1] + 6) s_nearest = obs[i,1] - idx_start = 10*(floor(obs[i,1]) - 1) - idx_end = 10*(floor(obs[i,1]) + 3) + idx_start = 16*(floor(obs[i,1]) - 1) + idx_end = 16*(floor(obs[i,1]) + 6) + + n_poly = 113 + + # if idx_start>n_nodes + # idx_start=idx_start%n_nodes + # idx_end=idx_end%n_nodes + # end - if idx_start>n_nodes - idx_start=idx_start%n_nodes - idx_end=idx_end%n_nodes - end if idx_start<=0 nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together @@ -1600,6 +1603,7 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra nodes_X = vec(nodes_XY[1,:]) nodes_Y = vec(nodes_XY[2,:]) + itp_matrix = zeros(n_poly,OrderXY+1) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 7fdfccdf..1c02c2f1 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -128,8 +128,8 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track obstacle.lap_active = 5 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [19] # initial s coordinate of each obstacle - obstacle.ey_obs_init = [-0.2] # initial ey coordinate of each obstacle + obstacle.s_obs_init = [18] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [0] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.2#0.5 obstacle.r_ey = 0.1#0.2 From 1a8ffa9864e1e8e2f94f912c0600882a93de6204 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 4 Dec 2017 10:17:11 -0800 Subject: [PATCH 127/183] modified scripts for connection with arduino and minor changes on the tuning --- env_loader_pc.sh | 14 +- eval_sim/eval_data.jl | 15 +- workspace/src/barc/launch/imu-remote.launch | 12 ++ workspace/src/barc/src/LMPC_node.jl | 131 ++++++++++++++---- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 56 ++++---- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 11 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 20 +-- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 2 +- 8 files changed, 180 insertions(+), 81 deletions(-) create mode 100644 workspace/src/barc/launch/imu-remote.launch diff --git a/env_loader_pc.sh b/env_loader_pc.sh index a9d68f9d..b5da0983 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -1,7 +1,9 @@ #!/bin/bash -source /opt/ros/kinetic/setup.bash -source /home/ugo/GitHub/barc/workspace/devel/setup.bash -export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=192.168.100.72 -export ROS_MASTER_URI=http://192.168.100.72:11311 -exec "$@" \ No newline at end of file +#source /opt/ros/kinetic/setup.bash +#source /home/ugo/GitHub/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/ugo/julia/bin/julia +#export ROS_IP=192.168.100.72 +#export ROS_MASTER_URI=http://192.168.100.72:11311 +export ROS_IP=10.0.0.14 +export ROS_MASTER_URI=http://10.0.0.14:11311 +exec "$@" diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index d9811de7..184593c4 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -167,12 +167,11 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cpsi2 = cpsi[1:currentIt,2,i] cpsi3 = cpsi[1:currentIt,3,i] - figure(1) - plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") - plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") - plot(pred_sol_xy[1],pred_sol_xy[2],) - plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - grid("on") + # figure(1) + # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + # plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + # grid("on") ellfig = figure(1) ax = ellfig[:add_subplot](1,1,1) @@ -290,6 +289,8 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("Overall velocity in lap $i") grid("on") + println("average velocity= ",mean(velocity)) + figure(6) @@ -1548,7 +1549,7 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra # println("obs= ",obs) OrderXY = 18 - OrderThetaCurv = 8 + OrderThetaCurv = 12 diff --git a/workspace/src/barc/launch/imu-remote.launch b/workspace/src/barc/launch/imu-remote.launch new file mode 100644 index 00000000..fc91c91d --- /dev/null +++ b/workspace/src/barc/launch/imu-remote.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index e18adecf..755fd23a 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -95,7 +95,7 @@ function main() mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) - #mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) mdl_obstacle = MpcModel_obstacle(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates,obstacle) @@ -276,7 +276,7 @@ function main() cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target #save the terminal cost for j = 1:buffersize - cost2target[j] = 1.5*(lapStatus.currentIt-j+1) + cost2target[j] = 1*(lapStatus.currentIt-j+1) end oldSS.cost2target[:,lapStatus.currentLap-1] = cost2target @@ -307,23 +307,34 @@ function main() elseif selectedStates.version == false && obstacle.obstacle_active == false - setvalue(mdl_convhull.z_Ol[:,1],zCurr[1,1]*ones(mpcParams.N+1))#mpcSol.z[:,4]) - setvalue(mdl_convhull.z_Ol[:,6],zCurr[1,6]*ones(mpcParams.N+1))#mpcSol.z[:,1]-posInfo.s_target) - setvalue(mdl_convhull.z_Ol[:,5],zCurr[1,5]*ones(mpcParams.N+1))#mpcSol.z[:,2]) - setvalue(mdl_convhull.z_Ol[:,4],zCurr[1,4]*ones(mpcParams.N+1))#mpcSol.z[:,3]) - setvalue(mdl_convhull.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) - setvalue(mdl_convhull.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,3]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,3]) + + #setvalue(mdl_convhull.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) + #setvalue(mdl_convhull.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) #setvalue(mdl_convhull.u_Ol,mpcSol.u[1:mpcParams.N,:]) setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) elseif selectedStates.version == false && obstacle.obstacle_active == true - setvalue(mdl_obstacle.z_Ol[:,1],zCurr[1,1]*ones(mpcParams.N+1))#mpcSol.z[:,4]) - setvalue(mdl_obstacle.z_Ol[:,6],zCurr[1,6]*ones(mpcParams.N+1))#mpcSol.z[:,1]-posInfo.s_target) - setvalue(mdl_obstacle.z_Ol[:,5],zCurr[1,5]*ones(mpcParams.N+1))#mpcSol.z[:,2]) - setvalue(mdl_obstacle.z_Ol[:,4],zCurr[1,4]*ones(mpcParams.N+1))#mpcSol.z[:,3]) - setvalue(mdl_obstacle.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) - setvalue(mdl_obstacle.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,4]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,4]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,2]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,2]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,3]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,3]) + + + # setvalue(mdl_obstacle.z_Ol[:,2],zCurr[1,2]*ones(mpcParams.N+1)) + # setvalue(mdl_obstacle.z_Ol[:,3],zCurr[1,3]*ones(mpcParams.N+1)) #setvalue(mdl_obstacle.u_Ol,mpcSol.u[1:mpcParams.N,:]) setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) @@ -335,12 +346,38 @@ function main() elseif selectedStates.version == false && obstacle.obstacle_active == false - setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]-posInfo.s_target) + + #setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) elseif selectedStates.version == false && obstacle.obstacle_active == true - setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]-posInfo.s_target) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]-posInfo.s_target) + + #setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) end end @@ -359,6 +396,48 @@ function main() mpcParams.Q_obs = ones(selectedStates.Nl*selectedStates.Np) + ############################################################## + #SSet warm start + if lapStatus.currentLap > n_pf && lapStatus.currentIt > 1 + if selectedStates.version == false && obstacle.obstacle_active == false + + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) + + #setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + + elseif selectedStates.version == false && obstacle.obstacle_active == true + + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) + setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) + setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) + + #setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + end + end + ############################################################## + if lapStatus.currentLap > 1 @@ -372,26 +451,26 @@ function main() if obstacle.obstacle_active == true - obs_temp = obs_curr[lapStatus.currentIt,:,:] + obs_temp = obs_curr[lapStatus.currentIt,:,:] - if posInfo.s_target-posInfo.s < obstacle.obs_detect # meaning that I could possibly detect obstacles after the finish line + if posInfo.s_target-posInfo.s < obstacle.obs_detect # meaning that I could possibly detect obstacles after the finish line - index1=find(obs_curr[lapStatus.currentIt,1,:].< obstacle.obs_detect+posInfo.s-posInfo.s_target) # look for obstacles that could cause problems + index1=find(obs_curr[lapStatus.currentIt,1,:].< obstacle.obs_detect+posInfo.s-posInfo.s_target) # look for obstacles that could cause problems - obs_temp[1,1,index1] = posInfo.s_target + obs_curr[lapStatus.currentIt,1,index1] + obs_temp[1,1,index1] = posInfo.s_target + obs_curr[lapStatus.currentIt,1,index1] - end + end - dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) + dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) - # println("dist= ",dist) - # println("obstacle's position= ",obs_temp[1,1,:]) - obs_near = obs_temp[1,:,index] - end + # println("dist= ",dist) + # println("obstacle's position= ",obs_temp[1,1,:]) + obs_near = obs_temp[1,:,index] + end diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 9ef8d1bb..aa79c3be 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -536,12 +536,12 @@ type MpcModel_convhull # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) # end - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) - for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) - end + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + # for i=1:N-1 # Constraints on u: + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + # end @@ -603,7 +603,7 @@ type MpcModel_convhull #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) - @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS) + @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS) #+ controlCost @@ -661,7 +661,7 @@ type MpcModel_test uPrev::Array{JuMP.NonlinearParameter,2} - #eps_lane::Array{JuMP.Variable,1} + eps_lane::Array{JuMP.Variable,1} #eps_alpha::Array{JuMP.Variable,1} #eps_vel::Array{JuMP.Variable,1} alpha::Array{JuMP.Variable,1} @@ -674,7 +674,7 @@ type MpcModel_test derivCost::JuMP.NonlinearExpression controlCost::JuMP.NonlinearExpression - #laneCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression terminalCost::JuMP.NonlinearExpression costPF::JuMP.NonlinearExpression #slackCost::JuMP.NonlinearExpression @@ -728,7 +728,7 @@ type MpcModel_test @variable( mdl, z_Ol[1:(N+1),1:7]) @variable( mdl, u_Ol[1:N,1:2]) - #@variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints + @variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha #@variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity @@ -769,8 +769,8 @@ type MpcModel_test @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i]) - #@NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) - #@NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull @@ -809,19 +809,19 @@ type MpcModel_test @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s end - # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) - # @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) - # for i=1:N-1 # Constraints on u: - # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) - # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) - # end + @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) + @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) + @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + end - # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) - # for i=1:N-1 # Constraints on u: - # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) - # end + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + end @@ -840,7 +840,7 @@ type MpcModel_test # Lane cost (soft) # --------------------------------- - #@NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) + @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) # Terminal Cost # --------------------------------- @@ -863,8 +863,8 @@ type MpcModel_test # Overall Cost function (objective of the minimization) # ----------------------------------------------------- - #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) - @NLobjective(mdl, Min, derivCost + controlCost + costPF + terminalCost) + @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + #@NLobjective(mdl, Min, derivCost + controlCost + costPF + terminalCost) sol_stat=solve(mdl) @@ -1136,7 +1136,7 @@ type MpcModel_obstacle # Soft Constraint on the Obstacle # -------------------------------- - @NLexpression(mdl, obstacleSlackCost, 1*sum{-log(((z_Ol[i,6]-obs[i,1])/r_s)^2 + ((z_Ol[i,5]-obs[i,2])/r_ey)^2 -1),i=1:N+1}) + @NLexpression(mdl, obstacleSlackCost, 0.1*sum{-log(((z_Ol[i,6]-obs[i,1])/r_s)^2 + ((z_Ol[i,5]-obs[i,2])/r_ey)^2 -1),i=1:N+1}) # Velocity Cost diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index 1c7575ed..b8ba556c 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -56,6 +56,10 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo selected_laps[i] = lapStatus.currentLap-i # use previous lap end + if lapStatus.currentLap >= 5 + selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2]) # and the best from all previous laps + end + # Select the old data oldxDot = oldTraj.oldTraj[:,1,selected_laps]::Array{Float64,3} oldyDot = oldTraj.oldTraj[:,2,selected_laps]::Array{Float64,3} @@ -96,7 +100,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! idx_s2= findmin(DistS2,1)[2] - off = 2 + off = 1 idx_s2 = idx_s2 + off # Propagate the obstacle for the prediction horizon @@ -118,10 +122,11 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo ellipse_check = (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,6]-obs_prop_s[n])/obstacle.r_s).^2) + (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,5]-obs_prop_ey[n])/obstacle.r_ey).^2) if any(x->x<=1, ellipse_check) == true # if any of the selected states is in the ellipse - + #println("flag**************************************************************************************************************") index = find(ellipse_check.<=1) # find all the states in the ellipse - #println("index= ",index[1]) + mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] = 10 # and set the values of the weight to 10, so that they are excluded from optimization + println("Q_obs= ",mpcParams.Q_obs) end end end diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 1c02c2f1..382e480f 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -28,7 +28,7 @@ function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{F if lapStatus.currentLap <= 2 # if it's the first or second lap oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything oldTraj.oldInput[:,:,1] = uCurr_export - oldTraj.oldTraj[:,:,2] = zCurr_export + oldTraj.oldTraj[:,:,2] = zCurr_export oldTraj.oldInput[:,:,2] = uCurr_export oldTraj.oldCost = [costLap,costLap] else # idea: always copy the new trajectory in the first array! @@ -47,26 +47,26 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 18 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 12 + mpcParams.N = 16 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) - mpcParams.vPathFollowing = 0.9 # reference speed for first lap of path following + mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,100.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 10 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = [20.0,1.0,10.0,20.0,50.0,50.0]#20.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = [20.0,10.0,10.0,30.0,80.0,50.0]#20.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories @@ -76,7 +76,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs - mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following + mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay @@ -125,11 +125,11 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem - obstacle.lap_active = 5 # number of the first lap in which the obstacles are used + obstacle.lap_active = 100 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [18] # initial s coordinate of each obstacle - obstacle.ey_obs_init = [0] # initial ey coordinate of each obstacle + obstacle.s_obs_init = [19] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.2#0.5 obstacle.r_ey = 0.1#0.2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index d1a8455c..69a46392 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -81,7 +81,7 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M z_ref1 = cat(2,zeros(mpcParams.N+1,3),v_ref*ones(mpcParams.N+1,1)) - z_ref2 = cat(2,zeros(mpcParams.N+1,1),0.1*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) + z_ref2 = cat(2,zeros(mpcParams.N+1,1),0.2*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) z_ref3 = cat(2,zeros(mpcParams.N+1,1),-0.1*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) sol_status::Symbol From f4497faf2be47956699c9696fb2c21ebcd86570b Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 4 Dec 2017 11:57:16 -0800 Subject: [PATCH 128/183] new tuning --- workspace/src/barc/src/LMPC_node.jl | 78 +++++++++---------- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 2 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 6 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 6 +- 4 files changed, 46 insertions(+), 46 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 755fd23a..95a8ee8f 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -397,45 +397,45 @@ function main() mpcParams.Q_obs = ones(selectedStates.Nl*selectedStates.Np) ############################################################## - #SSet warm start - if lapStatus.currentLap > n_pf && lapStatus.currentIt > 1 - if selectedStates.version == false && obstacle.obstacle_active == false - - setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) - setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) - setvalue(mdl_convhull.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) - setvalue(mdl_convhull.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) - setvalue(mdl_convhull.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) - setvalue(mdl_convhull.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) - setvalue(mdl_convhull.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) - setvalue(mdl_convhull.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) - - #setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) - - elseif selectedStates.version == false && obstacle.obstacle_active == true - - setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) - setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) - setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) - setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) - setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) - setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) - setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) - setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) - setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) - setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) - setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) - setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) - - #setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) - end - end + # #SSet warm start + # if lapStatus.currentLap > n_pf && lapStatus.currentIt > 1 + # if selectedStates.version == false && obstacle.obstacle_active == false + + # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) + # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) + # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) + # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) + # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) + # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) + # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) + # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) + # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) + # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) + # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) + # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) + + # #setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + # setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + + # elseif selectedStates.version == false && obstacle.obstacle_active == true + + # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) + # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) + # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) + # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) + # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) + # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) + # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) + # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) + # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) + # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) + # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) + # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) + + # #setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) + # setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) + # end + # end ############################################################## diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index b8ba556c..f4c49e44 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -100,7 +100,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! idx_s2= findmin(DistS2,1)[2] - off = 1 + off = 3 idx_s2 = idx_s2 + off # Propagate the obstacle for the prediction horizon diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 382e480f..f65c11be 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,14 +47,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.Np = 16 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 3 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 16 + mpcParams.N = 11 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index 69a46392..b884588f 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -93,11 +93,11 @@ function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::M setvalue(mdl.uPrev,uPrev) setvalue(mdl.coeff,coeffCurvature) if lapStatus.currentLap == 1 - setvalue(mdl.z_Ref,z_ref2) + setvalue(mdl.z_Ref,z_ref1) elseif lapStatus.currentLap == 2 - setvalue(mdl.z_Ref,z_ref2) + setvalue(mdl.z_Ref,z_ref1) elseif lapStatus.currentLap == 3 - setvalue(mdl.z_Ref,z_ref2) + setvalue(mdl.z_Ref,z_ref1) end # Solve Problem and return solution From aa52c0fd412fced447c7d452fa66fc76060f4985 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 7 Dec 2017 17:29:17 -0800 Subject: [PATCH 129/183] added env loader script and testing code for LMPC --- env_loader_odroid.sh | 8 ++-- eval_sim/eval_data.jl | 37 +++++++++++++------ odroid_setup.sh | 4 +- workspace/src/barc/launch/gps-remote.launch | 9 +++++ .../src/barc/launch/run_lmpc_remote.launch | 4 +- .../src/barc/launch/sensor_test_remote.launch | 4 +- workspace/src/barc/src/LMPC_node.jl | 2 +- .../src/barc/src/Localization_helpers.py | 32 ++++++++++------ .../src/barc/src/barc_lib/LMPC/functions.jl | 2 +- 9 files changed, 68 insertions(+), 34 deletions(-) create mode 100644 workspace/src/barc/launch/gps-remote.launch diff --git a/env_loader_odroid.sh b/env_loader_odroid.sh index d49076e1..da0b4a4f 100755 --- a/env_loader_odroid.sh +++ b/env_loader_odroid.sh @@ -2,7 +2,7 @@ source ~/barc/scripts/startup.sh source /opt/ros/indigo/setup.bash source ~/barc/workspace/devel/setup.bash -export PATH=$PATH:/home/odroid/julia -export ROS_IP=192.168.100.100 -export ROS_MASTER_URI=http://192.168.100.152:11311 -exec "$@" \ No newline at end of file +#export PATH=$PATH:/home/odroid/julia +export ROS_IP=10.0.0.1 +export ROS_MASTER_URI=http://10.0.0.14:11311 +exec "$@" diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 184593c4..676de54c 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1396,17 +1396,32 @@ function create_track(w) # add_curve(theta,35,0) # SIMPLE GOGGLE TRACK - add_curve(theta,30,0) - add_curve(theta,40,-pi/2) - add_curve(theta,10,0) - add_curve(theta,40,-pi/2) - add_curve(theta,20,pi/10) - add_curve(theta,30,-pi/5) - add_curve(theta,20,pi/10) - add_curve(theta,40,-pi/2) - add_curve(theta,10,0) - add_curve(theta,40,-pi/2) - add_curve(theta,35,0) + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,pi/10) + # add_curve(theta,30,-pi/5) + # add_curve(theta,20,pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + # OVAL TRACK FOR TESTS IN VSD + + add_curve(theta,80,0) + add_curve(theta,110,-pi) + add_curve(theta,160,0) + add_curve(theta,110,-pi) + add_curve(theta,80,0) + + # add_curve(theta,53,0) + # add_curve(theta,73,-pi) + # add_curve(theta,106,0) + # add_curve(theta,73,-pi) + # add_curve(theta,53,0) + # # SHORT SIMPLE track # add_curve(theta,10,0) diff --git a/odroid_setup.sh b/odroid_setup.sh index 5311301d..6201e3b0 100644 --- a/odroid_setup.sh +++ b/odroid_setup.sh @@ -1,2 +1,2 @@ -export ROS_MASTER_URI=http://192.168.100.72:11311 -export ROS_IP=192.168.100.72 \ No newline at end of file +export ROS_MASTER_URI=http://10.0.0.14:11311 +export ROS_IP=10.0.0.1 diff --git a/workspace/src/barc/launch/gps-remote.launch b/workspace/src/barc/launch/gps-remote.launch new file mode 100644 index 00000000..a151d23a --- /dev/null +++ b/workspace/src/barc/launch/gps-remote.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 7e53c1ae..a287abb8 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -7,8 +7,8 @@ - - + + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index 16c3c5eb..e1a5eff2 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -7,8 +7,8 @@ - - + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 95a8ee8f..17e3addb 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -167,7 +167,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 19.11#19.14#17.94#17.76#24.0 + posInfo.s_target = 10.8#19.11#19.14#17.94#17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index c1666956..f622754f 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -164,17 +164,27 @@ def create_track(self): # theta = add_curve(theta,37,0) # GOGGLE TRACK WITH STRAIGHT LINES, LENGTH = 19.11m (using ds = 0.03m) - theta = add_curve(theta,60,0) - theta = add_curve(theta,80,-pi/2) - theta = add_curve(theta,20,0) - theta = add_curve(theta,80,-pi/2) - theta = add_curve(theta,40,pi/10) - theta = add_curve(theta,60,-pi/5) - theta = add_curve(theta,40,pi/10) - theta = add_curve(theta,80,-pi/2) - theta = add_curve(theta,20,0) - theta = add_curve(theta,80,-pi/2) - theta = add_curve(theta,75,0) + # theta = add_curve(theta,60,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,40,pi/10) + # theta = add_curve(theta,60,-pi/5) + # theta = add_curve(theta,40,pi/10) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,75,0) + + # OVAL TRACK FOR TESTS IN VSD + + theta = add_curve(theta,43,0) + theta = add_curve(theta,146,-pi) + theta = add_curve(theta,86,0) + theta = add_curve(theta,146,-pi) + theta = add_curve(theta,43,0) + + #theta = add_curve(theta,33,0) # SHORT SIMPLE RACETRACK (smooth curves): 12.0m # theta = add_curve(theta,10,0) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index f65c11be..511e4add 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -56,7 +56,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.N = 11 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) - mpcParams.vPathFollowing = 1 # reference speed for first lap of path following + mpcParams.vPathFollowing = 0.5#1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states From 87b2eacb84ccb3b98c5260b0a65e71886d11e096 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 16 Jan 2018 10:28:27 -0800 Subject: [PATCH 130/183] setup for experiments --- .../arduino_nano328_node.ino | 28 +- .../src/barc/launch/run_lmpc_remote.launch | 3 + .../barc/launch/run_lmpc_remote.launch.orig | 54 ++ .../src/barc/launch/sensor_test_remote.launch | 9 +- .../src/barc/src/Localization_helpers.py | 76 ++- .../src/barc/src/barc_lib/LMPC/functions.jl | 8 +- .../src/barc/src/barc_lib/log_functions.jl | 2 +- workspace/src/barc/src/barc_record.jl | 2 + .../state_estimation_SensorKinematicModel.py | 54 +- .../src/view_car_trajectory-point-mass.py | 65 +++ workspace/src/barc/src/view_car_trajectory.py | 161 ++++++ .../src/ros_marvelmind_package/CHANGELOG.rst | 3 + .../src/ros_marvelmind_package/CMakeLists.txt | 2 + .../include/marvelmind_nav/marvelmind_hedge.h | 54 +- .../msg/beacon_pos_a.msg | 4 + .../ros_marvelmind_package/msg/hedge_pos.msg | 1 - .../msg/hedge_pos_a.msg | 6 + .../src/ros_marvelmind_package/package.xml | 2 +- .../src/hedge_rcv_bin.cpp | 102 +++- .../src/marvelmind_hedge.c | 523 +++++++++++++++--- .../src/subscriber_test.cpp | 71 ++- 21 files changed, 1079 insertions(+), 151 deletions(-) create mode 100644 workspace/src/barc/launch/run_lmpc_remote.launch.orig create mode 100755 workspace/src/barc/src/view_car_trajectory-point-mass.py create mode 100755 workspace/src/barc/src/view_car_trajectory.py create mode 100644 workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg create mode 100644 workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index 6b4d85d3..c2dfeb2d 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -133,8 +133,8 @@ class Car { int BL_count = 0; int BR_count = 0; - // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) - // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) + // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) + // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) volatile unsigned long FL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) volatile unsigned long FR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) volatile unsigned long BL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) @@ -145,10 +145,10 @@ class Car { volatile unsigned long BL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) volatile unsigned long BR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) - unsigned long FL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) - unsigned long FR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) - unsigned long BL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) - unsigned long BR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long FL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long FR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) // Utility functions @@ -157,6 +157,9 @@ class Car { float saturateServo(float x); }; +// Boolean keeping track of whether the Arduino has received a signal from the ECU recently +int received_ecu_signal = 0; + // Initialize an instance of the Car class as car Car car; @@ -166,6 +169,7 @@ Car car; // figure it out, please atone for my sins. void ecuCallback(const barc::ECU& ecu) { car.writeToActuators(ecu); + received_ecu_signal = 1; } void incFLCallback() { car.incFL(); @@ -189,6 +193,7 @@ void calcThrottleCallback() { // Variables for time step volatile unsigned long dt; volatile unsigned long t0; +volatile unsigned long ecu_t0; // Global message variables // Encoder, RC Inputs, Electronic Control Unit, Ultrasound @@ -228,6 +233,7 @@ void setup() // Arming ESC, 1 sec delay for arming and ROS car.armActuators(); t0 = millis(); + ecu_t0 = millis(); } @@ -239,6 +245,16 @@ void loop() { // compute time elapsed (in ms) dt = millis() - t0; + // kill the motor if there is no ECU signal within the last 1s + if( (millis() - ecu_t0) >= 200){ + if(!received_ecu_signal){ + car.killMotor(); + } else{ + received_ecu_signal = 0; + } + ecu_t0 = millis(); + } + if (dt > 50) { car.readAndCopyInputs(); diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index a287abb8..f8eee292 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -36,6 +36,9 @@ + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch.orig b/workspace/src/barc/launch/run_lmpc_remote.launch.orig new file mode 100644 index 00000000..7e53c1ae --- /dev/null +++ b/workspace/src/barc/launch/run_lmpc_remote.launch.orig @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index e1a5eff2..7dd6fbd2 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -8,16 +8,17 @@ - + - + + - + diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index f622754f..c46092cc 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -164,7 +164,51 @@ def create_track(self): # theta = add_curve(theta,37,0) # GOGGLE TRACK WITH STRAIGHT LINES, LENGTH = 19.11m (using ds = 0.03m) - # theta = add_curve(theta,60,0) + theta = add_curve(theta,60,0) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,20,0) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,40,pi/10) + theta = add_curve(theta,60,-pi/5) + theta = add_curve(theta,40,pi/10) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,20,0) + theta = add_curve(theta,80,-pi/2) + theta = add_curve(theta,75,0) + + + ###################################################### + # TEST TRACKS + + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,10,0) + + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,230,0) + + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,10,0) + + # theta = add_curve(theta,40,-pi/2) + # theta = add_curve(theta,205,0) + + + + # theta = add_curve(theta,130,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,40,pi/10) + # theta = add_curve(theta,60,-pi/5) + # theta = add_curve(theta,40,pi/10) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,5,0) + + + # theta = add_curve(theta,10,0) # theta = add_curve(theta,80,-pi/2) # theta = add_curve(theta,20,0) # theta = add_curve(theta,80,-pi/2) @@ -174,24 +218,27 @@ def create_track(self): # theta = add_curve(theta,80,-pi/2) # theta = add_curve(theta,20,0) # theta = add_curve(theta,80,-pi/2) - # theta = add_curve(theta,75,0) + # theta = add_curve(theta,125,0) + - # OVAL TRACK FOR TESTS IN VSD + # theta = add_curve(theta,10,0) + # theta = add_curve(theta,60,-pi/2) + # theta = add_curve(theta,60,-pi/2) + # theta = add_curve(theta,50,0) + # theta = add_curve(theta,60,-pi/2) + # theta = add_curve(theta,60,-pi/2) + # theta = add_curve(theta,40,0) - theta = add_curve(theta,43,0) - theta = add_curve(theta,146,-pi) - theta = add_curve(theta,86,0) - theta = add_curve(theta,146,-pi) - theta = add_curve(theta,43,0) + ############################################################ - #theta = add_curve(theta,33,0) # SHORT SIMPLE RACETRACK (smooth curves): 12.0m + + # theta = add_curve(theta,5,0) + # theta = add_curve(theta,40,-pi) # theta = add_curve(theta,10,0) - # theta = add_curve(theta,80,-pi) - # theta = add_curve(theta,20,0) - # theta = add_curve(theta,80,-pi) - # theta = add_curve(theta,9,0) + # theta = add_curve(theta,40,-pi) + # theta = add_curve(theta,4,0) # SIMPLER RACETRACK (half circles as curves): @@ -199,6 +246,7 @@ def create_track(self): x = hstack((x, x[-1] + cos(theta[i])*ds)) y = hstack((y, y[-1] + sin(theta[i])*ds)) + self.nodes = array([x,y]) self.ds = ds self.n = size(x) @@ -287,6 +335,8 @@ def find_s(self): dist = sum((self.pos*ones([self.n,2])-self.nodes.transpose())**2,1)**0.5 # distance of current position to all nodes idx_min = argmin(dist) # index of minimum distance + + n = self.n # number of nodes nPoints = self.nPoints # number of points for polynomial approximation (around current position) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 511e4add..5420734d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -56,7 +56,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.N = 11 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) - mpcParams.vPathFollowing = 0.5#1 # reference speed for first lap of path following + mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states @@ -72,11 +72,11 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.N = 16 - mpcParams_pF.Q = [0.0,50.0,0.1,10.0] + mpcParams_pF.Q = [0.0,50.0,10.1,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states - mpcParams_pF.QderivU = 1.0*[10,10] # cost matrix for derivative cost of inputs - mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following + mpcParams_pF.QderivU = 1*[10,10] # cost matrix for derivative cost of inputs + mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay diff --git a/workspace/src/barc/src/barc_lib/log_functions.jl b/workspace/src/barc/src/barc_lib/log_functions.jl index 78195fb8..a56bf241 100644 --- a/workspace/src/barc/src/barc_lib/log_functions.jl +++ b/workspace/src/barc/src/barc_lib/log_functions.jl @@ -50,7 +50,7 @@ end function GPS_callback(msg::hedge_pos,gps_meas::Measurements) gps_meas.t[gps_meas.i] = to_sec(get_rostime()) - gps_meas.t_msg[gps_meas.i] = to_sec(msg.header.stamp) + gps_meas.t_msg[gps_meas.i] = msg.timestamp_ms gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] gps_meas.i += 1 nothing diff --git a/workspace/src/barc/src/barc_record.jl b/workspace/src/barc/src/barc_record.jl index 33e59ffe..c7d66fc5 100755 --- a/workspace/src/barc/src/barc_record.jl +++ b/workspace/src/barc/src/barc_record.jl @@ -11,11 +11,13 @@ Position info (/pos_info) using RobotOS @rosimport barc.msg: ECU, pos_info, Vel_est +#@rosimport data_service.msg: TimeData @rosimport geometry_msgs.msg: Vector3 @rosimport sensor_msgs.msg: Imu @rosimport marvelmind_nav.msg: hedge_pos rostypegen() using barc.msg +#using data_service.msg using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 57957331..1eaa33c7 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -50,8 +50,6 @@ class StateEst(object): # Velocity vel_meas = 0.0 vel_updated = False - vel_prev = 0.0 - vel_count = 0.0 # this counts how often the same vel measurement has been received # GPS x_meas = 0.0 @@ -85,26 +83,50 @@ def ecu_callback(self, data): def gps_callback(self, data): """This function is called when a new GPS signal is received.""" # units: [rad] and [rad/s] + # get current time stamp t_now = rospy.get_rostime().to_sec()-self.t0 - t_msg = data.header.stamp.to_sec()-self.t0 + #t_msg = data.timestamp_ms/1000.0 - self.t0 + t_msg = t_now + # if abs(t_now - t_msg) > 0.1: # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) + + # get current gps measurement self.x_meas = data.x_m self.y_meas = data.y_m + #print "Received coordinates : (%f, %f)" % (self.x_meas, self.y_meas) + + # check if we have good measurement + # compute distance we have travelled from previous estimate to current measurement + # if we've travelled more than 1 m, the GPS measurement is probably junk, so ignore it + # otherwise, store measurement, and then perform interpolation dist = (self.x_est-data.x_m)**2 + (self.y_est-data.y_m)**2 if dist < 1.0: self.x_hist = append(self.x_hist, data.x_m) self.y_hist = append(self.y_hist, data.y_m) self.t_gps = append(self.t_gps, t_msg) + # self.x_hist = delete(self.x_hist,0) # self.y_hist = delete(self.y_hist,0) # self.t_gps = delete(self.t_gps,0) + + # Keep only the last second worth of coordinate data in the x_hist and y_hist buffer + # These buffers are used for interpolation + # without overwriting old data, the arrays would grow unbounded self.x_hist = self.x_hist[self.t_gps > t_now-1.0] self.y_hist = self.y_hist[self.t_gps > t_now-1.0] self.t_gps = self.t_gps[self.t_gps > t_now-1.0] sz = size(self.t_gps, 0) + + # perform interpolation for (x,y) as a function of time t + # getting two function approximations x(t) and y(t) + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # use least squares to get the coefficients for this function approximation + # using (x,y) coordinate data from the past second (time) if sz > 4: - t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T + t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T # input matrix: [ t^2 t 1 ] self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] self.gps_updated = True @@ -146,18 +168,9 @@ def imu_callback(self, data): self.att = (roll_raw,pitch_raw,yaw_raw) self.imu_updated = True - def vel_est_callback(self, data): - #self.vel_meas = (data.vel_fl+data.vel_fr)/2.0#data.vel_est - if data.vel_est != self.vel_prev: - self.vel_meas = data.vel_est - self.vel_updated = True - self.vel_prev = data.vel_est - self.vel_count = 0 - else: - self.vel_count = self.vel_count + 1 - if self.vel_count > 10: # if 10 times in a row the same measurement - self.vel_meas = 0 # set velocity measurement to zero - self.vel_updated = True + def encoder_vel_callback(self, data): + self.vel_meas = data.vel_est + self.vel_updated = True # state estimation node def state_estimation(): @@ -167,7 +180,7 @@ def state_estimation(): # topic subscriptions / publications rospy.Subscriber('imu/data', Imu, se.imu_callback) - rospy.Subscriber('vel_est', Vel_est, se.vel_est_callback) + rospy.Subscriber('vel_est', Vel_est, se.encoder_vel_callback) rospy.Subscriber('ecu', ECU, se.ecu_callback) rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback, queue_size=1) state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) @@ -235,10 +248,14 @@ def state_estimation(): bta = 0.5 * u[1] + # print "V, V_x and V_y : (%f, %f, %f)" % (se.vel_meas,cos(bta)*se.vel_meas, sin(bta)*se.vel_meas) + # get measurement y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) + + # build extra arguments for non-linear function args = (u, vhMdl, dt, 0) @@ -250,10 +267,13 @@ def state_estimation(): se.x_est = x_est_2 se.y_est = y_est_2 + #print "V_x and V_y : (%f, %f)" % (v_x_est, v_y_est) # Update track position l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) + #l.set_pos(se.x_meas, se.y_meas, psi_est_2, v_x_est, v_y_est, psi_dot_est) + # Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz) if est_counter%4 == 0: l.find_s() diff --git a/workspace/src/barc/src/view_car_trajectory-point-mass.py b/workspace/src/barc/src/view_car_trajectory-point-mass.py new file mode 100755 index 00000000..79f73f4d --- /dev/null +++ b/workspace/src/barc/src/view_car_trajectory-point-mass.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Author: J. Noonan +# Email: jpnoonan@berkeley.edu +# +# This code provides a way to see the car's trajectory, orientation, and velocity profile in +# real time with referenced to the track defined a priori. +# +# --------------------------------------------------------------------------- + +import rospy +from marvelmind_nav.msg import hedge_pos +import matplotlib.pyplot as plt +import pylab + +x_gps = None +y_gps = None + +def gps_callback(data): + global x_gps, y_gps + x_gps = data.x_m + y_gps = data.y_m + +def show(): + plt.show() + +def view_trajectory(): + global x_gps, y_gps + + rospy.init_node("car_view_trajectory_node", anonymous=True) + rospy.Subscriber("hedge_pos", hedge_pos, gps_callback) + rospy.on_shutdown(show) + + fig = pylab.figure() + ax1 = fig.add_subplot(1, 1, 1) + ax1.axis('equal') + ax1.grid('on') + pylab.ion() + xlim = 5 + ylim = 5 + + loop_rate = 50 + rate = rospy.Rate(loop_rate) + + while not rospy.is_shutdown(): + if (x_gps and y_gps): + ax1.plot(x_gps, y_gps, 'bo', label="GPS data path") + ax1.set_xlim([-xlim, xlim]) + ax1.set_ylim([-ylim, ylim]) + + ax1.set_title("Data from indoor GPS") + pylab.pause(0.001) + #pylab.gcf().clear() + +if __name__ == '__main__': + try: + view_trajectory() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/view_car_trajectory.py b/workspace/src/barc/src/view_car_trajectory.py new file mode 100755 index 00000000..a747e363 --- /dev/null +++ b/workspace/src/barc/src/view_car_trajectory.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Author: J. Noonan +# Email: jpnoonan@berkeley.edu +# +# This code provides a way to see the car's trajectory, orientation, and velocity profile in +# real time with referenced to the track defined a priori. +# +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append, ones, polyval, delete, size, empty, linspace +from numpy import ones, polyval, delete, size +from observers import ekf +from system_models import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math +import matplotlib.pyplot as plt +import numpy as np +import pylab + +global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev +global pos_info_x_vals, pos_info_y_vals +global v_vals, t_vals, psi_vals + +gps_x_vals = [] +gps_y_vals = [] +gps_x_prev = 0.0 +gps_y_prev = 0.0 + +pos_info_x_vals = [] +pos_info_y_vals = [] + +v_vals = [] +t_vals = [] +psi_curr = 0.0 + +def gps_callback(data): + global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev + + dist = (gps_x_prev - data.x_m)**2 + (gps_y_prev - data.y_m)**2 + if dist < 1: + gps_x_vals.append(data.x_m) + gps_y_vals.append(data.y_m) + gps_x_prev = data.x_m + gps_y_prev = data.y_m + else: + gps_x_vals.append(gps_x_prev) + gps_y_vals.append(gps_y_prev) + + +def pos_info_callback(data): + global pos_info_x_vals, pos_info_y_vals + global v_vals, t_vals, psi_curr + + pos_info_x_vals.append(data.x) + pos_info_y_vals.append(data.y) + + v_vals.append(data.v) + t_vals.append(rospy.get_rostime().to_sec()) + psi_curr = data.psi + +def show(): + plt.show() + +def view_trajectory(): + + global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev + global pos_info_x_vals, pos_info_y_vals + global v_vals, t_vals, psi_curr + + rospy.init_node("car_view_trajectory_node", anonymous=True) + rospy.on_shutdown(show) + + rospy.Subscriber("hedge_pos", hedge_pos, gps_callback) + rospy.Subscriber("pos_info", pos_info, pos_info_callback) + + l = Localization() + l.create_track() + #l.create_circle(rad=0.8, c=array([0.0, -0.5])) + + fig = pylab.figure() + pylab.ion() + + vmax_ref = 1.0 + + loop_rate = 50 + rate = rospy.Rate(loop_rate) + + car_dx = 0.306 + car_dy = 0.177 + + car_xs_origin = [car_dx, car_dx, -car_dx, -car_dx, car_dx] + car_ys_origin = [car_dy, -car_dy, -car_dy, car_dy, car_dy] + + car_frame = np.vstack((np.array(car_xs_origin), np.array(car_ys_origin))) + while not rospy.is_shutdown(): + ax1 = fig.add_subplot(2, 1, 1) + ax2 = fig.add_subplot(2, 1, 2) + ax1.plot(l.nodes[0,:],l.nodes[1,:],"r-o") + ax1.grid('on') + ax1.axis('equal') + + + if (gps_x_vals and gps_y_vals): + ax1.plot(gps_x_vals[:len(gps_x_vals)-1], gps_y_vals[:len(gps_y_vals)-1], 'b-', label="GPS data path") + ax1.plot(gps_x_vals[len(gps_x_vals)-1], gps_y_vals[len(gps_y_vals)-1], 'b*',label="Car current GPS Pos") + + if (pos_info_x_vals and pos_info_y_vals): + ax1.plot(pos_info_x_vals[:len(pos_info_x_vals)-1], pos_info_y_vals[:len(pos_info_y_vals)-1], 'g-', label="Car path") + + x = pos_info_x_vals[len(pos_info_x_vals)-1] + y = pos_info_y_vals[len(pos_info_y_vals)-1] + ax1.plot(x, y, 'gs', label="Car current pos") + + R = np.matrix([[np.cos(psi_curr), -np.sin(psi_curr)], [np.sin(psi_curr), np.cos(psi_curr)]]) + + rotated_car_frame = R * car_frame + + car_xs = np.array(rotated_car_frame[0,:])[0] + car_ys = np.array(rotated_car_frame[1,:])[0] + + front_car_segment_x = np.array([car_xs[0], car_xs[1]]) + x + front_car_segment_y = np.array([car_ys[0], car_ys[1]]) + y + + ax1.plot(car_xs[1:] + x, car_ys[1:] + y, 'k-') + ax1.plot(front_car_segment_x, front_car_segment_y, 'y-') + #plt.plot(np.array(car_xs_origin) + x, np.array(car_ys_origin) + y, 'k-') + + if (v_vals): + t_vals_zeroed = [t - t_vals[0] for t in t_vals] + ax2.plot(t_vals_zeroed, v_vals, 'm-') + ax2.set_ylim([min(0, min(v_vals)), max(vmax_ref, max(v_vals))]) + + + ax1.set_title("Green = Data from POS_INFO, Blue = Data from GPS") + + ax2.set_xlabel("Time (s)") + ax2.set_ylabel("Velocity (m/s)") + + pylab.pause(0.001) + pylab.gcf().clear() + + + +if __name__ == '__main__': + try: + view_trajectory() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/ros_marvelmind_package/CHANGELOG.rst b/workspace/src/ros_marvelmind_package/CHANGELOG.rst index 93323d0f..b89c12a0 100644 --- a/workspace/src/ros_marvelmind_package/CHANGELOG.rst +++ b/workspace/src/ros_marvelmind_package/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package marvelmind_nav ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.6 (2017-02-10) +------------------ + 1.0.5 (2016-09-15) ------------------ diff --git a/workspace/src/ros_marvelmind_package/CMakeLists.txt b/workspace/src/ros_marvelmind_package/CMakeLists.txt index 77b706d6..41cc267b 100644 --- a/workspace/src/ros_marvelmind_package/CMakeLists.txt +++ b/workspace/src/ros_marvelmind_package/CMakeLists.txt @@ -51,6 +51,8 @@ add_message_files( DIRECTORY msg FILES hedge_pos.msg + hedge_pos_a.msg + beacon_pos_a.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h b/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h index 7888296e..8eb798f5 100644 --- a/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h +++ b/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h @@ -5,9 +5,32 @@ struct PositionValue { + uint8_t address; uint32_t timestamp; - int16_t x, y, z; + int32_t x, y, z;// coordinates in millimeters uint8_t flags; + + bool highResolution; + + bool ready; + bool processed; +}; + +struct StationaryBeaconPosition +{ + uint8_t address; + int32_t x, y, z;// coordinates in millimeters + + bool updatedForMsg; + bool highResolution; +}; +#define MAX_STATIONARY_BEACONS 30 +struct StationaryBeaconsPositions +{ + uint8_t numBeacons; + struct StationaryBeaconPosition beacons[MAX_STATIONARY_BEACONS]; + + bool updated; }; struct MarvelmindHedge @@ -28,6 +51,8 @@ struct MarvelmindHedge // buffer of measurements struct PositionValue * positionBuffer; + + struct StationaryBeaconsPositions positionsBeacons; // verbose flag which activate console output // default: False @@ -44,6 +69,7 @@ struct MarvelmindHedge // private variables uint8_t lastValuesCount_; + uint8_t lastValues_next; bool haveNewValues_; #ifdef WIN32 HANDLE thread_; @@ -54,28 +80,26 @@ struct MarvelmindHedge #endif }; -#define POSITION_DATAGRAM_HEADER {0xff, 0x47, 0x01, 0x00, 0x10} -#define POSITION_DATAGRAM_HEADER_SIZE 5 - -#pragma pack (push, 1) -struct PositionDatagram_ -{ - uint8_t header[POSITION_DATAGRAM_HEADER_SIZE]; - uint32_t timestamp; - int16_t x, y, z; - uint8_t flags; - uint8_t align[5]; - uint16_t crc; -}; -#pragma pack (pop) +#define POSITION_DATAGRAM_ID 0x0001 +#define BEACONS_POSITIONS_DATAGRAM_ID 0x0002 +#define POSITION_DATAGRAM_HIGHRES_ID 0x0011 +#define BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012 struct MarvelmindHedge * createMarvelmindHedge (); void destroyMarvelmindHedge (struct MarvelmindHedge * hedge); void startMarvelmindHedge (struct MarvelmindHedge * hedge); + void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, bool onlyNew); bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, struct PositionValue * position); + +void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew); +bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct StationaryBeaconsPositions * positions); +void clearStationaryBeaconUpdatedFlag(struct MarvelmindHedge * hedge, uint8_t address); + void stopMarvelmindHedge (struct MarvelmindHedge * hedge); #ifdef WIN32 diff --git a/workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg b/workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg new file mode 100644 index 00000000..2f71c076 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg @@ -0,0 +1,4 @@ +uint8 address +float64 x_m +float64 y_m +float64 z_m diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg index 1d141d04..84a917dc 100644 --- a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg @@ -1,4 +1,3 @@ -Header header int64 timestamp_ms float64 x_m float64 y_m diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg new file mode 100644 index 00000000..1c826020 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg @@ -0,0 +1,6 @@ +uint8 address +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +uint8 flags diff --git a/workspace/src/ros_marvelmind_package/package.xml b/workspace/src/ros_marvelmind_package/package.xml index ab001a4d..a9aafe22 100644 --- a/workspace/src/ros_marvelmind_package/package.xml +++ b/workspace/src/ros_marvelmind_package/package.xml @@ -1,7 +1,7 @@ marvelmind_nav - 1.0.5 + 1.0.6 Marvelmind local navigation system diff --git a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp index ceebfde7..ecdf1656 100644 --- a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp +++ b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp @@ -4,6 +4,8 @@ #include "ros/ros.h" #include "std_msgs/String.h" #include "marvelmind_nav/hedge_pos.h" +#include "marvelmind_nav/hedge_pos_a.h" +#include "marvelmind_nav/beacon_pos_a.h" extern "C" { #include "marvelmind_nav/marvelmind_hedge.h" @@ -12,12 +14,16 @@ extern "C" #include #define ROS_NODE_NAME "hedge_rcv_bin" -#define POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_ADDRESSED_TOPIC_NAME "/hedge_pos_a" +#define BEACONS_POSITION_ADDRESSED_TOPIC_NAME "/beacons_pos_a" struct MarvelmindHedge * hedge= NULL; static uint32_t hedge_timestamp_prev= 0; -marvelmind_nav::hedge_pos hedge_pos_msg;// message for publishing to ROS topic +marvelmind_nav::hedge_pos hedge_pos_noaddress_msg;// hedge coordinates message (old version without address) for publishing to ROS topic +marvelmind_nav::hedge_pos_a hedge_pos_msg;// hedge coordinates message for publishing to ROS topic +marvelmind_nav::beacon_pos_a beacon_pos_msg;// stationary beacon coordinates message for publishing to ROS topic //////////////////////////////////////////////////////////////////////// @@ -47,30 +53,77 @@ static bool hedgeReceiveCheck(void) struct PositionValue position; getPositionFromMarvelmindHedge (hedge, &position); + hedge_pos_msg.address= position.address; + hedge_pos_msg.flags= position.flags; + hedge_pos_noaddress_msg.flags= position.flags; if (hedge_pos_msg.flags&(1<<1))// flag of timestamp format { hedge_pos_msg.timestamp_ms= position.timestamp;// msec + hedge_pos_noaddress_msg.timestamp_ms= position.timestamp; } else { hedge_pos_msg.timestamp_ms= position.timestamp*15.625;// alpha-cycles ==> msec + hedge_pos_noaddress_msg.timestamp_ms= position.timestamp*15.625; } - hedge_pos_msg.header.stamp= ros::Time::now(); // added by mb for custom hedge_pos msg - hedge_pos_msg.x_m= position.x/100.0; - hedge_pos_msg.y_m= position.y/100.0; - hedge_pos_msg.z_m= position.z/100.0; + hedge_pos_msg.x_m= position.x/1000.0; + hedge_pos_msg.y_m= position.y/1000.0; + hedge_pos_msg.z_m= position.z/1000.0; + + hedge_pos_noaddress_msg.x_m= position.x/1000.0; + hedge_pos_noaddress_msg.y_m= position.y/1000.0; + hedge_pos_noaddress_msg.z_m= position.z/1000.0; hedge->haveNewValues_=false; + + return true; } + return false; +} + +static bool beaconReceiveCheck(void) +{ + uint8_t i; + struct StationaryBeaconsPositions positions; + struct StationaryBeaconPosition *bp= NULL; + bool foundUpd= false; + uint8_t n; + + getStationaryBeaconsPositionsFromMarvelmindHedge (hedge, &positions); + n= positions.numBeacons; + if (n == 0) + return false; + + for(i=0;iupdatedForMsg) + { + clearStationaryBeaconUpdatedFlag(hedge, bp->address); + foundUpd= true; + break; + } + } + if (!foundUpd) + return false; + if (bp == NULL) + return false; + + beacon_pos_msg.address= bp->address; + beacon_pos_msg.x_m= bp->x/1000.0; + beacon_pos_msg.y_m= bp->y/1000.0; + beacon_pos_msg.z_m= bp->z/1000.0; + + return true; } /** * Node for Marvelmind hedgehog binary streaming data processing */ int main(int argc, char **argv) -{ +{uint8_t beaconReadIterations; // initialize ROS node ros::init(argc, argv, ROS_NODE_NAME); @@ -80,18 +133,32 @@ int main(int argc, char **argv) // ROS node reference ros::NodeHandle n; - // Register topic for puplishing messages - ros::Publisher hedge_pos_publisher = n.advertise(POSITION_TOPIC_NAME, 1000); + // Register topics for puplishing messages + ros::Publisher hedge_pos_publisher = n.advertise(HEDGE_POSITION_ADDRESSED_TOPIC_NAME, 1000); + ros::Publisher hedge_pos_noaddress_publisher = n.advertise(HEDGE_POSITION_TOPIC_NAME, 1000); + ros::Publisher beacons_pos_publisher = n.advertise(BEACONS_POSITION_ADDRESSED_TOPIC_NAME, 1000); // 50 Hz ros::Rate loop_rate(50); // default values for position message + hedge_pos_msg.address= 0; hedge_pos_msg.timestamp_ms = 0; hedge_pos_msg.x_m = 0.0; hedge_pos_msg.y_m = 0.0; hedge_pos_msg.z_m = 0.0; hedge_pos_msg.flags = (1<<0);// 'data not available' flag + + hedge_pos_noaddress_msg.timestamp_ms = 0; + hedge_pos_noaddress_msg.x_m = 0.0; + hedge_pos_noaddress_msg.y_m = 0.0; + hedge_pos_noaddress_msg.z_m = 0.0; + hedge_pos_noaddress_msg.flags = (1<<0);// 'data not available' flag + + beacon_pos_msg.address= 0; + beacon_pos_msg.x_m = 0.0; + beacon_pos_msg.y_m = 0.0; + beacon_pos_msg.z_m = 0.0; while (ros::ok()) @@ -103,16 +170,29 @@ int main(int argc, char **argv) if (hedgeReceiveCheck()) {// hedgehog data received - ROS_INFO("%d, %d, %.3f, X=%.3f Y= %.3f Z=%.3f flags=%d", + ROS_INFO("Address: %d, timestamp: %d, %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.address, (int) hedge_pos_msg.timestamp_ms, (int) (hedge_pos_msg.timestamp_ms - hedge_timestamp_prev), - (float) hedge_pos_msg.header.stamp.toSec(), (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, (int) hedge_pos_msg.flags); hedge_pos_publisher.publish(hedge_pos_msg); + hedge_pos_noaddress_publisher.publish(hedge_pos_noaddress_msg); hedge_timestamp_prev= hedge_pos_msg.timestamp_ms; } + + beaconReadIterations= 0; + while(beaconReceiveCheck()) + {// stationary beacons data received + ROS_INFO("Stationary beacon: Address: %d, X=%.3f Y= %.3f Z=%.3f", + (int) beacon_pos_msg.address, + (float) beacon_pos_msg.x_m, (float) beacon_pos_msg.y_m, (float) beacon_pos_msg.z_m); + beacons_pos_publisher.publish(beacon_pos_msg); + + if ((beaconReadIterations++)>4) + break; + } ros::spinOnce(); diff --git a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c index 9bd2b103..e3544d08 100644 --- a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c +++ b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c @@ -12,8 +12,6 @@ #endif // WIN32 #include "marvelmind_nav/marvelmind_hedge.h" -uint8_t positionDatagramHeader[]=POSITION_DATAGRAM_HEADER; - ////////////////////////////////////////////////////////////////////////////// // Calculate CRC (Modbus) for array of bytes // buf: input buffer @@ -93,7 +91,7 @@ HANDLE OpenSerialPort_ (const char * portFileName, uint32_t baudrate, if (verbose) puts ("Error: unable to set serial port parameters"); CloseHandle (ttyHandle); return INVALID_HANDLE_VALUE; - } + } return ttyHandle; } #else @@ -218,6 +216,203 @@ int OpenSerialPort_ (const char * portFileName, uint32_t baudrate, bool verbose) } #endif +////////////////////////////////////////////////////////////////////////// + +static uint8_t markPositionReady(struct MarvelmindHedge * hedge) +{uint8_t ind= hedge->lastValues_next; + uint8_t indCur= ind; + + hedge->positionBuffer[ind].ready= + true; + hedge->positionBuffer[ind].processed= + false; + ind++; + if (ind>=hedge->maxBufferedPositions) + ind=0; + if (hedge->lastValuesCount_maxBufferedPositions) + hedge->lastValuesCount_++; + hedge->haveNewValues_=true; + + hedge->lastValues_next= ind; + + return indCur; +} + +static struct PositionValue process_position_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t ind= hedge->lastValues_next; + + hedge->positionBuffer[ind].address= + buffer[16]; + hedge->positionBuffer[ind].timestamp= + buffer[5] | + (((uint32_t ) buffer[6])<<8) | + (((uint32_t ) buffer[7])<<16) | + (((uint32_t ) buffer[8])<<24); + + int16_t vx= buffer[9] | + (((uint16_t ) buffer[10])<<8); + hedge->positionBuffer[ind].x= vx*10;// millimeters + + int16_t vy= buffer[11] | + (((uint16_t ) buffer[12])<<8); + hedge->positionBuffer[ind].y= vy*10;// millimeters + + int16_t vz= buffer[13] | + (((uint16_t ) buffer[14])<<8); + hedge->positionBuffer[ind].z= vz*10;// millimeters + + hedge->positionBuffer[ind].flags= buffer[15]; + + hedge->positionBuffer[ind].highResolution= false; + + ind= markPositionReady(hedge); + + return hedge->positionBuffer[ind]; +} + +static struct PositionValue process_position_highres_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t ind= hedge->lastValues_next; + + hedge->positionBuffer[ind].address= + buffer[22]; + hedge->positionBuffer[ind].timestamp= + buffer[5] | + (((uint32_t ) buffer[6])<<8) | + (((uint32_t ) buffer[7])<<16) | + (((uint32_t ) buffer[8])<<24); + + int32_t vx= buffer[9] | + (((uint32_t ) buffer[10])<<8) | + (((uint32_t ) buffer[11])<<16) | + (((uint32_t ) buffer[12])<<24); + hedge->positionBuffer[ind].x= vx; + + int32_t vy= buffer[13] | + (((uint32_t ) buffer[14])<<8) | + (((uint32_t ) buffer[15])<<16) | + (((uint32_t ) buffer[16])<<24); + hedge->positionBuffer[ind].y= vy; + + int32_t vz= buffer[17] | + (((uint32_t ) buffer[18])<<8) | + (((uint32_t ) buffer[19])<<16) | + (((uint32_t ) buffer[20])<<24); + hedge->positionBuffer[ind].z= vz; + + hedge->positionBuffer[ind].flags= buffer[21]; + + hedge->positionBuffer[ind].highResolution= true; + + ind= markPositionReady(hedge); + + return hedge->positionBuffer[ind]; +} + +static struct StationaryBeaconPosition *getOrAllocBeacon(struct MarvelmindHedge * hedge,uint8_t address) +{ + uint8_t i; + uint8_t n_used= hedge->positionsBeacons.numBeacons; + + if (n_used != 0) + for(i=0;ipositionsBeacons.beacons[i].address == address) + { + return &hedge->positionsBeacons.beacons[i]; + } + } + + if (n_used >= (MAX_STATIONARY_BEACONS-1)) + return NULL; + + hedge->positionsBeacons.numBeacons= (n_used + 1); + return &hedge->positionsBeacons.beacons[n_used]; +} + +static void process_beacons_positions_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{ + uint8_t n= buffer[5];// number of beacons in packet + uint8_t i,ofs; + uint8_t address; + int16_t x,y,z; + struct StationaryBeaconPosition *b; + + if ((1+n*8)!=buffer[4]) + return;// incorrect size + + for(i=0;iaddress= address; + b->x= x*10;// millimeters + b->y= y*10;// millimeters + b->z= z*10;// millimeters + + b->highResolution= false; + b->updatedForMsg= true; + + hedge->positionsBeacons.updated= true; + } + } +} + +static void process_beacons_positions_highres_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{ + uint8_t n= buffer[5];// number of beacons in packet + uint8_t i,ofs; + uint8_t address; + int32_t x,y,z; + struct StationaryBeaconPosition *b; + + if ((1+n*14)!=buffer[4]) + return;// incorrect size + + for(i=0;iaddress= address; + b->x= x; + b->y= y; + b->z= z; + + b->highResolution= true; + b->updatedForMsg= true; + + hedge->positionsBeacons.updated= true; + } + } +} + enum @@ -229,6 +424,7 @@ enum ////////////////////////////////////////////////////////////////////////////// // Thread function started by MarvelmindHedge_start ////////////////////////////////////////////////////////////////////////////// + void #ifndef WIN32 * @@ -236,11 +432,11 @@ void Marvelmind_Thread_ (void* param) { struct MarvelmindHedge * hedge=(struct MarvelmindHedge*) param; - struct PositionDatagram_ positionDatagram; + struct PositionValue curPosition; + uint8_t input_buffer[256]; uint8_t recvState=RECV_HDR; // current state of receive data uint8_t nBytesInBlockReceived=0; // bytes received - - uint8_t lastValues_next=0; + uint16_t dataId; SERIAL_PORT_HANDLE ttyHandle=OpenSerialPort_(hedge->ttyFileName, hedge->baudRate, hedge->verbose); @@ -253,7 +449,7 @@ Marvelmind_Thread_ (void* param) uint8_t receivedChar; bool readSuccessed=true; #ifdef WIN32 - uint32_t nBytesRead; + DWORD nBytesRead; readSuccessed=ReadFile (ttyHandle, &receivedChar, 1, &nBytesRead, NULL); #else int32_t nBytesRead; @@ -262,59 +458,90 @@ Marvelmind_Thread_ (void* param) #endif if (nBytesRead && readSuccessed) { - //printf("%x %d %d ", receivedChar & 0xff, nBytesRead, readSuccessed); - - *((char*)&positionDatagram+nBytesInBlockReceived)=receivedChar; + bool goodByte= false; + input_buffer[nBytesInBlockReceived]= receivedChar; switch (recvState) { case RECV_HDR: - if (receivedChar==positionDatagramHeader[nBytesInBlockReceived]) + switch(nBytesInBlockReceived) + { + case 0: + goodByte= (receivedChar == 0xff); + break; + case 1: + goodByte= (receivedChar == 0x47); + break; + case 2: + goodByte= true; + break; + case 3: + dataId= (((uint16_t) receivedChar)<<8) + input_buffer[2]; + goodByte= (dataId == POSITION_DATAGRAM_ID) || + (dataId == BEACONS_POSITIONS_DATAGRAM_ID) || + (dataId == POSITION_DATAGRAM_HIGHRES_ID) || + (dataId == BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID); + break; + case 4: + switch(dataId ) + { + case POSITION_DATAGRAM_ID: + goodByte= (receivedChar == 0x10); + break; + case BEACONS_POSITIONS_DATAGRAM_ID: + case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID: + goodByte= true; + break; + case POSITION_DATAGRAM_HIGHRES_ID: + goodByte= (receivedChar == 0x16); + break; + } + if (goodByte) + recvState=RECV_DGRAM; + break; + } + if (goodByte) { // correct header byte nBytesInBlockReceived++; - if (nBytesInBlockReceived>=sizeof(positionDatagramHeader)) - { - recvState=RECV_DGRAM; - } } else { // ...or incorrect + recvState=RECV_HDR; nBytesInBlockReceived=0; } break; case RECV_DGRAM: nBytesInBlockReceived++; - if (nBytesInBlockReceived>=sizeof(struct PositionDatagram_)) + if (nBytesInBlockReceived>=7+input_buffer[4]) { // parse dgram uint16_t blockCrc= - CalcCrcModbus_((uint8_t*)&positionDatagram, - sizeof(struct PositionDatagram_)-2); - if (blockCrc==positionDatagram.crc) + CalcCrcModbus_(input_buffer,nBytesInBlockReceived); + if (blockCrc==0) { - // add to positionBuffer #ifdef WIN32 EnterCriticalSection(&hedge->lock_); #else pthread_mutex_lock (&hedge->lock_); #endif - hedge->positionBuffer[lastValues_next].timestamp= - positionDatagram.timestamp; - hedge->positionBuffer[lastValues_next].x= - positionDatagram.x; - hedge->positionBuffer[lastValues_next].y= - positionDatagram.y; - hedge->positionBuffer[lastValues_next].z= - positionDatagram.z; - hedge->positionBuffer[lastValues_next].flags= - positionDatagram.flags; - lastValues_next++; - if (lastValues_next>=hedge->maxBufferedPositions) - lastValues_next=0; - if (hedge->lastValuesCount_maxBufferedPositions) - hedge->lastValuesCount_++; - hedge->haveNewValues_=true; + switch(dataId ) + { + case POSITION_DATAGRAM_ID: + // add to positionBuffer + curPosition= process_position_datagram(hedge, input_buffer); + break; + case BEACONS_POSITIONS_DATAGRAM_ID: + process_beacons_positions_datagram(hedge, input_buffer); + break; + case POSITION_DATAGRAM_HIGHRES_ID: + // add to positionBuffer + curPosition= process_position_highres_datagram(hedge, input_buffer); + break; + case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID: + process_beacons_positions_highres_datagram(hedge, input_buffer); + break; + } #ifdef WIN32 LeaveCriticalSection(&hedge->lock_); #else @@ -323,15 +550,10 @@ Marvelmind_Thread_ (void* param) // callback if (hedge->receiveDataCallback) { - struct PositionValue position= + if (dataId == POSITION_DATAGRAM_ID) { - positionDatagram.timestamp, - positionDatagram.x, - positionDatagram.y, - positionDatagram.z, - positionDatagram.flags - }; - hedge->receiveDataCallback (position); + hedge->receiveDataCallback (curPosition); + } } } // and repeat @@ -362,6 +584,7 @@ struct MarvelmindHedge * createMarvelmindHedge () hedge->verbose=false; hedge->receiveDataCallback=NULL; hedge->lastValuesCount_=0; + hedge->lastValues_next= 0; hedge->haveNewValues_=false; hedge->terminationRequired= false; #ifdef WIN32 @@ -378,7 +601,8 @@ struct MarvelmindHedge * createMarvelmindHedge () // Initialize and start work thread ////////////////////////////////////////////////////////////////////////////// void startMarvelmindHedge (struct MarvelmindHedge * hedge) -{ +{uint8_t i; + hedge->positionBuffer= malloc(sizeof (struct PositionValue)*hedge->maxBufferedPositions); if (hedge->positionBuffer==NULL) @@ -387,6 +611,18 @@ void startMarvelmindHedge (struct MarvelmindHedge * hedge) hedge->terminationRequired=true; return; } + for(i=0;imaxBufferedPositions;i++) + { + hedge->positionBuffer[i].ready= false; + hedge->positionBuffer[i].processed= false; + } + for(i=0;ipositionsBeacons.beacons[i].updatedForMsg= false; + } + hedge->positionsBeacons.numBeacons= 0; + hedge->positionsBeacons.updated= false; + #ifdef WIN32 _beginthread (Marvelmind_Thread_, 0, hedge); #else @@ -400,14 +636,16 @@ void startMarvelmindHedge (struct MarvelmindHedge * hedge) // position: pointer to PositionValue for write coordinates // returncode: true if position is valid ////////////////////////////////////////////////////////////////////////////// -bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, - struct PositionValue * position) + +static bool getPositionFromMarvelmindHedgeByAddress (struct MarvelmindHedge * hedge, + struct PositionValue * position, uint8_t address) { uint8_t i; - int16_t avg_x=0, avg_y=0, avg_z=0; + int32_t avg_x=0, avg_y=0, avg_z=0; uint32_t max_timestamp=0; - uint8_t flags= 0; bool position_valid; + bool highRes= false; + uint8_t flags= 0; #ifdef WIN32 EnterCriticalSection(&hedge->lock_); #else @@ -416,29 +654,47 @@ bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, if (hedge->lastValuesCount_) { uint8_t real_values_count=hedge->maxBufferedPositions; + uint8_t nFound= 0; if (hedge->lastValuesCount_lastValuesCount_; - position_valid=true; for (i=0; ipositionBuffer[i].address != address) + continue; + if (!hedge->positionBuffer[i].ready) + continue; + if (hedge->positionBuffer[i].processed) + continue; + if (address == 0) + address= hedge->positionBuffer[i].address; + nFound++; avg_x+=hedge->positionBuffer[i].x; avg_y+=hedge->positionBuffer[i].y; avg_z+=hedge->positionBuffer[i].z; - if (hedge->positionBuffer[i].timestamp>max_timestamp) - max_timestamp=hedge->positionBuffer[i].timestamp; - + flags= hedge->positionBuffer[i].flags; if (flags&(1<<0)) { position_valid=false; } + + if (hedge->positionBuffer[i].highResolution) + highRes= true; + hedge->positionBuffer[i].processed= true; + if (hedge->positionBuffer[i].timestamp>max_timestamp) + max_timestamp=hedge->positionBuffer[i].timestamp; + } + if (nFound != 0) + { + avg_x/=nFound; + avg_y/=nFound; + avg_z/=nFound; + position_valid=true; + } else + { + position_valid=false; } - avg_x/=real_values_count; - avg_y/=real_values_count; - avg_z/=real_values_count; - - if (!position_valid) - flags|= (1<<0);// coordiantes not available } else position_valid=false; #ifdef WIN32 @@ -446,28 +702,167 @@ bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, #else pthread_mutex_unlock (&hedge->lock_); #endif + if (!position_valid) + { + flags|= (1<<0);// coordiantes not available + } + position->address= address; position->x=avg_x; position->y=avg_y; position->z=avg_z; position->timestamp=max_timestamp; + position->ready= position_valid; + position->highResolution= highRes; position->flags= flags; return position_valid; } +bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct PositionValue * position) +{ + return getPositionFromMarvelmindHedgeByAddress(hedge, position, 0); +}; + ////////////////////////////////////////////////////////////////////////////// // Print average position coordinates // onlyNew: print only new positions ////////////////////////////////////////////////////////////////////////////// void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, bool onlyNew) -{ +{uint8_t i,j; + double xm,ym,zm; + if (hedge->haveNewValues_ || (!onlyNew)) { struct PositionValue position; - getPositionFromMarvelmindHedge (hedge, &position); - printf ("X: %d, Y: %d, Z: %d at time T: %u\n", position.x, - position.y, position.z, position.timestamp); - hedge->haveNewValues_=false; + uint8_t real_values_count=hedge->maxBufferedPositions; + uint8_t addresses[real_values_count]; + uint8_t addressesNum= 0; + + for(i=0;ipositionBuffer[i].address; + bool alreadyProcessed= false; + if (addressesNum != 0) + for(j=0;jhaveNewValues_=false; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Get positions of stationary beacons +// hedge: MarvelmindHedge structure +// positions: pointer to structure for write coordinates +////////////////////////////////////////////////////////////////////////////// + +bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct StationaryBeaconsPositions * positions) +{ +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + + *positions= hedge->positionsBeacons; + +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + + return true; +} + +void clearStationaryBeaconUpdatedFlag(struct MarvelmindHedge * hedge, uint8_t address) +{uint8_t i,n; + +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + + n= hedge->positionsBeacons.numBeacons; + + for(i=0;ipositionsBeacons.beacons[i].address == address) + { + hedge->positionsBeacons.beacons[i].updatedForMsg= false; + } + } + +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Print stationary beacons positions +// onlyNew: print only new positions +////////////////////////////////////////////////////////////////////////////// +void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew) +{struct StationaryBeaconsPositions positions; + double xm,ym,zm; + + getStationaryBeaconsPositionsFromMarvelmindHedge(hedge, &positions); + + if (positions.updated || (!onlyNew)) + {uint8_t i; + uint8_t n= hedge->positionsBeacons.numBeacons; + struct StationaryBeaconPosition *b; + + for(i=0;ix)/1000.0; + ym= ((double) b->y)/1000.0; + zm= ((double) b->z)/1000.0; + if (positions.beacons[i].highResolution) + { + printf ("Stationary beacon: address: %d, X: %.3f, Y: %.3f, Z: %.3f \n", + b->address,xm, ym, zm); + } else + { + printf ("Stationary beacon: address: %d, X: %.2f, Y: %.2f, Z: %.2f \n", + b->address,xm, ym, zm); + } + } + + hedge->positionsBeacons.updated= false; } } diff --git a/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp b/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp index b03d4ac8..c0edff17 100644 --- a/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp +++ b/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp @@ -1,19 +1,26 @@ #include "ros/ros.h" #include "std_msgs/String.h" #include "marvelmind_nav/hedge_pos.h" +#include "marvelmind_nav/hedge_pos_a.h" +#include "marvelmind_nav/beacon_pos_a.h" #include #define ROS_NODE_NAME "subscriber_test" #define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_ADDRESSED_TOPIC_NAME "/hedge_pos_a" +#define BEACONS_POSITION_ADDRESSED_TOPIC_NAME "/beacons_pos_a" #define SCALE_HEDGE 3.0 ros::Publisher rviz_marker_pub; + uint32_t rviz_shape; +typedef enum {objHedge, objBeacon} DrawObjectType; -void showRvizObject(float x, float y, float z) -{ +void showRvizObject(uint8_t address, float x, float y, float z, DrawObjectType obj) +{uint8_t lifeTime, i; + if (rviz_marker_pub.getNumSubscribers() < 1) return; visualization_msgs::Marker marker; @@ -24,7 +31,7 @@ void showRvizObject(float x, float y, float z) // Set the namespace and id for this marker. This serves to create a unique ID // Any marker sent with the same namespace and id will overwrite the old one marker.ns = "basic_shapes"; - marker.id = 0; + marker.id = address; // Set the marker type marker.type = rviz_shape; @@ -46,29 +53,63 @@ void showRvizObject(float x, float y, float z) marker.scale.y = 0.05*SCALE_HEDGE; marker.scale.z = 0.02*SCALE_HEDGE; // Set the color -- be sure to set alpha to something non-zero! - marker.color.r = 0.0f; - marker.color.g = 1.0f; - marker.color.b = 0.0f; + if (obj == objHedge) + { + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + } + else + { + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + } marker.color.a = 1.0; - marker.lifetime = ros::Duration(5); + if (obj == objHedge) lifeTime= 5; + else lifeTime= 25; + marker.lifetime = ros::Duration(lifeTime); rviz_marker_pub.publish(marker); } -void hedgePosCallback(const marvelmind_nav::hedge_pos& hedge_pos_msg) +void hedgePosCallback(const marvelmind_nav::hedge_pos_a& hedge_pos_msg) +{ + ROS_INFO("Hedgehog data: Address= %d, timestamp= %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.address, + (int) hedge_pos_msg.timestamp_ms, + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (int) hedge_pos_msg.flags); + + if ((hedge_pos_msg.flags&(1<<0))==0) + { + showRvizObject(hedge_pos_msg.address,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); + } +} + +void hedgePos_noaddressCallback(const marvelmind_nav::hedge_pos& hedge_pos_msg) { - ROS_INFO("Hedgehog data: %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + ROS_INFO("Hedgehog data: Timestamp= %d, X=%.3f Y= %.3f Z=%.3f flags=%d", (int) hedge_pos_msg.timestamp_ms, (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, (int) hedge_pos_msg.flags); if ((hedge_pos_msg.flags&(1<<0))==0) { - showRvizObject(hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m); + showRvizObject(0,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); } } +void beaconsPosCallback(const marvelmind_nav::beacon_pos_a& beacon_pos_msg) +{ + ROS_INFO("Stationary beacon data: Address= %d, X=%.3f Y= %.3f Z=%.3f", + (int) beacon_pos_msg.address, + (float) beacon_pos_msg.x_m, (float) beacon_pos_msg.y_m, (float) beacon_pos_msg.z_m); + + showRvizObject(beacon_pos_msg.address, beacon_pos_msg.x_m, beacon_pos_msg.y_m, beacon_pos_msg.z_m, objBeacon); +} + /** * Test subscriber node for getting data from Marvelmind publishers nodes */ @@ -77,15 +118,17 @@ int main(int argc, char **argv) // initialize ROS node ros::init(argc, argv, ROS_NODE_NAME); - + // ROS node reference - ros::NodeHandle n; + ros::NodeHandle rosNode; // Declare need to subscribe data from topic - ros::Subscriber sub = n.subscribe(HEDGE_POSITION_TOPIC_NAME, 1000, hedgePosCallback); + ros::Subscriber subHedge = rosNode.subscribe(HEDGE_POSITION_ADDRESSED_TOPIC_NAME, 1000, hedgePosCallback); + //ros::Subscriber subHedge_noaddress = rosNode.subscribe(HEDGE_POSITION_TOPIC_NAME, 1000, hedgePos_noaddressCallback); + ros::Subscriber subBeacons = rosNode.subscribe(BEACONS_POSITION_ADDRESSED_TOPIC_NAME, 1000, beaconsPosCallback); // Declare publisher for rviz visualization - rviz_marker_pub = n.advertise("visualization_marker", 1); + rviz_marker_pub = rosNode.advertise("visualization_marker", 1); // Set our initial shape type to be a cube rviz_shape = visualization_msgs::Marker::CUBE; From b3b679b22362a746788a41ceeca36689919a1629 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 16 Jan 2018 17:32:11 -0800 Subject: [PATCH 131/183] minor changes --- eval_sim/eval_data.jl | 32 ++++++++++---------- workspace/src/barc/src/LMPC_node.jl | 16 +++++----- workspace/src/barc/src/barc_simulator_dyn.jl | 2 +- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 676de54c..c934e645 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1396,25 +1396,25 @@ function create_track(w) # add_curve(theta,35,0) # SIMPLE GOGGLE TRACK - # add_curve(theta,30,0) - # add_curve(theta,40,-pi/2) - # add_curve(theta,10,0) - # add_curve(theta,40,-pi/2) - # add_curve(theta,20,pi/10) - # add_curve(theta,30,-pi/5) - # add_curve(theta,20,pi/10) - # add_curve(theta,40,-pi/2) - # add_curve(theta,10,0) - # add_curve(theta,40,-pi/2) - # add_curve(theta,35,0) + add_curve(theta,30,0) + add_curve(theta,40,-pi/2) + add_curve(theta,10,0) + add_curve(theta,40,-pi/2) + add_curve(theta,20,pi/10) + add_curve(theta,30,-pi/5) + add_curve(theta,20,pi/10) + add_curve(theta,40,-pi/2) + add_curve(theta,10,0) + add_curve(theta,40,-pi/2) + add_curve(theta,35,0) # OVAL TRACK FOR TESTS IN VSD - add_curve(theta,80,0) - add_curve(theta,110,-pi) - add_curve(theta,160,0) - add_curve(theta,110,-pi) - add_curve(theta,80,0) + # add_curve(theta,80,0) + # add_curve(theta,110,-pi) + # add_curve(theta,160,0) + # add_curve(theta,110,-pi) + # add_curve(theta,80,0) # add_curve(theta,53,0) # add_curve(theta,73,-pi) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 17e3addb..d5372406 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -95,7 +95,7 @@ function main() mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) - mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + #mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) mdl_obstacle = MpcModel_obstacle(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates,obstacle) @@ -167,7 +167,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 10.8#19.11#19.14#17.94#17.76#24.0 + posInfo.s_target = 19.11#19.14#17.94#17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -254,12 +254,12 @@ function main() # ============================= Pre-Logging (before solving) ================================ log_t[k+1] = to_sec(get_rostime()) # time is measured *before* solving (more consistent that way) - # if size(mpcSol.z,2) == 4 # find 1-step-error - # step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]) - # else - # step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]) - # end - # log_step_diff[k+1,:] = step_diff + if size(mpcSol.z,2) == 4 # find 1-step-error + step_diff = ([mpcSol.z[2,4], 0, 0, mpcSol.z[2,3], mpcSol.z[2,2]]-[norm(zCurr[i,1:2]), 0, 0, zCurr[i,4], zCurr[i,5]]) + else + step_diff = (mpcSol.z[2,1:5][:]-zCurr[i,1:5][:]) + end + log_step_diff[k+1,:] = step_diff if lapStatus.currentLap > n_pf if lapStatus.currentIt>1 diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl index 8516ea0d..4599498d 100755 --- a/workspace/src/barc/src/barc_simulator_dyn.jl +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -239,7 +239,7 @@ function main() gps_meas.t[gps_meas.i] = t gps_meas.z[gps_meas.i,:] = [x y] gps_meas.i += 1 - gps_data.header.stamp = get_rostime() + # gps_data.header.stamp = get_rostime() gps_data.x_m = x gps_data.y_m = y publish(pub_gps, gps_data) From a68092990f33927e9205a2ff1d45791c83665ee6 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 18 Jan 2018 09:20:38 -0800 Subject: [PATCH 132/183] new tuning --- workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index aa79c3be..5afeaf64 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -536,12 +536,12 @@ type MpcModel_convhull # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) # end - # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) - # for i=1:N-1 # Constraints on u: - # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) - # end + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + end From 19998371ac41540a95b3ade94eb3209145092300 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 18 Jan 2018 09:31:48 -0800 Subject: [PATCH 133/183] small changes to plots for debugging --- eval_sim/eval_data.jl | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index c934e645..aad28488 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -167,24 +167,24 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cpsi2 = cpsi[1:currentIt,2,i] cpsi3 = cpsi[1:currentIt,3,i] - # figure(1) - # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") - # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") - # plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - # grid("on") - - ellfig = figure(1) - ax = ellfig[:add_subplot](1,1,1) - ax[:set_aspect]("equal") + figure(1) plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + grid("on") + + # ellfig = figure(1) + # ax = ellfig[:add_subplot](1,1,1) + # ax[:set_aspect]("equal") + # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + # plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) - angle_deg = (angle_ell*180)/pi + # angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + # angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 0)#angle=angle_deg) - ax[:add_artist](ell1) + # ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 0)#angle=angle_deg) + # ax[:add_artist](ell1) grid("on") From 80fddcbb0a6655df39b55eb29acc5f4f6cc5a0bc Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 22 Jan 2018 09:45:51 -0800 Subject: [PATCH 134/183] added codes for mapping of accelleration --- .../Parameter estimation/find_a_mapping.jl | 12 +- eval_sim/mapping_francesco.jl | 80 ++++ eval_sim/test_load.jl | 89 +++++ .../src/barc/launch/open_loop_remote.launch | 4 +- workspace/src/barc/src/open_loop.jl | 370 +++++++++++++----- workspace/src/barc/src/open_loop_francesco.jl | 117 ++++++ 6 files changed, 569 insertions(+), 103 deletions(-) create mode 100644 eval_sim/mapping_francesco.jl create mode 100644 eval_sim/test_load.jl create mode 100644 workspace/src/barc/src/open_loop_francesco.jl diff --git a/eval_sim/Parameter estimation/find_a_mapping.jl b/eval_sim/Parameter estimation/find_a_mapping.jl index eb869d2f..fe6dde2a 100644 --- a/eval_sim/Parameter estimation/find_a_mapping.jl +++ b/eval_sim/Parameter estimation/find_a_mapping.jl @@ -27,14 +27,14 @@ function main(code::AbstractString) cmd = zeros(length(t)) for i=1:length(t) - #v[i] = vel_est.z[t[i].>vel_est.t,1][end] - v[i] = pos_info.z[t[i].>pos_info.t,15][end] + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] end v_opt = v[1:end] cmd_opt = cmd[1:end] - v_opt = v[cmd.>94] - cmd_opt = cmd[cmd.>94] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] res = optimize(c->cost(cmd_opt,v_opt,c,0),[0.001,0,0.1,0.001,0]) c = Optim.minimizer(res) @@ -63,8 +63,8 @@ function v_over_u(code::AbstractString) psiDot = zeros(length(t)) for i=1:length(t) - #v[i] = vel_est.z[t[i].>vel_est.t,1][end] - v[i] = pos_info.z[t[i].>pos_info.t,15][end] + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] cmd[i,:] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,:][end,:] psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] end diff --git a/eval_sim/mapping_francesco.jl b/eval_sim/mapping_francesco.jl new file mode 100644 index 00000000..7fcd9bb5 --- /dev/null +++ b/eval_sim/mapping_francesco.jl @@ -0,0 +1,80 @@ + +using PyPlot +using JLD +#using Optim + +# type Measurements{T} +# i::Int64 # measurement counter +# t::Array{Float64} # time data (when it was received by this recorder) +# t_msg::Array{Float64} # time that the message was sent +# z::Array{T} # measurement values +# end + +function data_analysis(code::AbstractString) + + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + # t_cmd = d_rec["t_cmd"] + # t_no_cmd = d_rec["t_no_cmd"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + figure() + plot(v_opt) + + # half = round(length(v_opt)/2) + # println(half) + + #v_mean = mean(v_opt[half:end]) + + #v_mean = mean(v_opt[length(v_opt)/2:end]) + + #a = v_mean/4 + + a = v_opt[end]/4 + + println("a= ",a) + + figure(2) + + plot(cmd_opt[1],a, "*") + + + # println(t_cmd) + # println(t_no_cmd) + # println(t0) + + # figure() + # plot(cmd,v) + + # figure() + # plot(t,cmd) + + # figure() + # plot(t,v) + + # figure() + # plot(v) + + # figure() + # plot(t) +end \ No newline at end of file diff --git a/eval_sim/test_load.jl b/eval_sim/test_load.jl new file mode 100644 index 00000000..5487c012 --- /dev/null +++ b/eval_sim/test_load.jl @@ -0,0 +1,89 @@ +using PyPlot +using JLD + +function test(path::AbstractString) + + + files = readdir(path) + + a = zeros(length(files)) + + v_final = zeros(length(files)) + + commands = zeros(length(files)) + + for index = 1:length(files) + + file = files[index] + + log_path = "$(homedir())/open_loop/$file" + + d_rec=load(log_path) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + figure(1) + + plot(v_opt) + + # println("length= ",length(files)) + # println("v_opt= ",v_opt) + # println("v_opt[end]= ",v_opt[end]) + # println("a[i]= ",a[i]) + # println("v_opt[end]/4= ",v_opt[end]/4) + + v_final[index] = v_opt[end] + a[index] = v_opt[end]/4 + commands[index]=cmd_opt[end] + + + println("a= ",a) + println("number of tests= ",length(a)) + + figure(2) + + plot(cmd_opt[1],a[index], "*") + + end + + # Least Mean Square + + A = ones(length(files),3) + B = zeros(length(files),1) + x = zeros(3,1) #[c1_+;c2_+,cM] + + A[:,1] = commands + A[:,3] = -v_final + + B = a + + x = inv(A'*A)*A'*B + + println("****quick check of the results****") + + a_test = x[1]*commands[1] + x[2] - x[3]*v_final[1] + + println("a_test= ",a_test) + + +end diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch index 11ca8aa9..7322c583 100644 --- a/workspace/src/barc/launch/open_loop_remote.launch +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -7,8 +7,8 @@ - - + + diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index b41c3d94..05839014 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -1,5 +1,196 @@ #!/usr/bin/env julia + +########################################################################################################################################################################## +########################################################################################################################################################################## + +# using RobotOS +# @rosimport barc.msg: ECU, pos_info, Vel_est +# @rosimport geometry_msgs.msg: Vector3 +# @rosimport sensor_msgs.msg: Imu +# @rosimport marvelmind_nav.msg: hedge_pos +# rostypegen() +# using barc.msg +# using geometry_msgs.msg +# using sensor_msgs.msg +# using std_msgs.msg +# using marvelmind_nav.msg +# using JLD +# include("barc_lib/log_functions.jl") + +# function main() + +# # Set up logging +# buffersize = 60000 +# gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) +# imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) +# cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) +# cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) +# vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) +# pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,20)) + +# # Initialize ROS node and topics +# init_node("mpc_traj") +# s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} +# s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} +# s3 = Subscriber("ecu_pwm", ECU, ECU_PWM_callback, (cmd_pwm_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} +# s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} +# s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} +# s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} + +# run_id = get_param("run_id") + +# loop_rate = Rate(10) +# pub = Publisher("ecu_pwm", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} +# cmd = ECU() # command type +# cmd.motor = 0 +# cmd.servo = 0 +# cmd.header.stamp = get_rostime() +# println("Starting open loop control. Waiting.") +# rossleep(3.0) + +# t0_ros = get_rostime() +# t0 = to_sec(t0_ros) +# t = 0.0 + +# gps_meas.i = 1 +# imu_meas.i = 1 +# cmd_log.i = 1 +# cmd_pwm_log.i = 1 +# vel_est_log.i = 1 +# pos_info_log.i = 1 + +# chicane_speed = 97#1.0 +# chicane_turn = [120,60] + +# cmd_m = 94 +# cmd_s = 80 +# t_next = 7 +# mode = 1 # 1 = acc, 2 = break, 3 = break +# # Idea: 5 seconds acc, 3 seconds breaking. + +# # Start node +# while ! is_shutdown()#t < 29.0 # run exactly x seconds +# t = to_sec(get_rostime())-t0 +# if t <= 3 +# cmd.motor = 90.0 +# cmd.servo = 80.0 +# elseif t <= t_next +# if mode == 1 +# cmd.motor = cmd_m +# cmd.servo = cmd_s +# elseif mode == 2 +# cmd.motor = 70 +# end +# elseif t <= 300 +# if mode == 1 +# mode = 2 +# else +# cmd_s += 0 +# cmd_m += 1 +# mode = 1 +# end +# t_next = t + 4 +# # ************************************** +# # CHICANE: +# # elseif t <= 3 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[1] +# # elseif t <= 4 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[2] +# # elseif t <= 5 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[1] +# # elseif t <= 6 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[2] +# # elseif t <= 7 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[1] +# # elseif t <= 8 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[2] +# # elseif t <= 9 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[1] +# # elseif t <= 10 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[2] +# # elseif t <= 11 +# # cmd.motor = chicane_speed +# # cmd.servo = chicane_turn[1] +# # ************************************** +# # CONTINUOUS ACCELERATION: +# # elseif t <= 123 +# # cmd.motor = 0.0+(t-3)/20 +# # cmd.servo = 0.0#-(t-3.0)/300-0.15 +# # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = 100.0 +# # elseif t <= 13 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = 105.0 +# # elseif t <= 18 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = 110.0 +# # elseif t <= 23 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = 115.0 +# # elseif t <= 28 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = 120.0 +# # elseif t <= 33 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = -0.4 +# # elseif t <= 38 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = -0.45 +# # elseif t <= 43 # CHECK TIME AND ACCELERATION !!! +# # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! +# # cmd.servo = -0.5 +# # elseif t <= 44 +# # cmd.motor = -2.0 +# # cmd.servo = 0 +# else +# cmd.motor = 70.0 +# cmd.servo = 90.0 +# end +# cmd.header.stamp = get_rostime() +# publish(pub, cmd) + +# rossleep(loop_rate) +# end + +# # Clean up buffers +# clean_up(gps_meas) +# clean_up(imu_meas) +# clean_up(cmd_log) +# clean_up(cmd_pwm_log) +# clean_up(pos_info_log) +# clean_up(vel_est_log) + +# println("Exiting open loop control.") +# log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4]).jld" +# if isfile(log_path) +# log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4])-2.jld" +# warn("Warning: File already exists.") +# end +# save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) +# println("Exiting node... Saved recorded data to $log_path.") +# end + +# if ! isinteractive() +# main() +# end + + +###################################################################################################################################################################### +###################################################################################################################################################################### +###################################################################################################################################################################### + + + using RobotOS @rosimport barc.msg: ECU, pos_info, Vel_est @rosimport geometry_msgs.msg: Vector3 @@ -11,6 +202,7 @@ using geometry_msgs.msg using sensor_msgs.msg using std_msgs.msg using marvelmind_nav.msg +using PyPlot using JLD include("barc_lib/log_functions.jl") @@ -49,116 +241,97 @@ function main() t0 = to_sec(t0_ros) t = 0.0 + t_next = 4 + gps_meas.i = 1 imu_meas.i = 1 cmd_log.i = 1 cmd_pwm_log.i = 1 vel_est_log.i = 1 pos_info_log.i = 1 - - chicane_speed = 97#1.0 - chicane_turn = [120,60] - cmd_m = 94 - cmd_s = 80 - t_next = 7 - mode = 1 # 1 = acc, 2 = break, 3 = break - # Idea: 5 seconds acc, 3 seconds breaking. + cmd_m = 102 + + # t_cmd = zeros(10) + # t_no_cmd = zeros(10) - # Start node - while ! is_shutdown()#t < 29.0 # run exactly x seconds + # index=1 + + # time_index = 0 + + # a=zeros(10) + + + while ! is_shutdown() # run exactly x seconds t = to_sec(get_rostime())-t0 - if t <= 3 - cmd.motor = 90.0 - cmd.servo = 80.0 - elseif t <= t_next - if mode == 1 - cmd.motor = cmd_m - cmd.servo = cmd_s - elseif mode == 2 - cmd.motor = 70 - end - elseif t <= 300 - if mode == 1 - mode = 2 - else - cmd_s += 0 - cmd_m += 1 - mode = 1 - end - t_next = t + 4 - # ************************************** - # CHICANE: - # elseif t <= 3 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[1] - # elseif t <= 4 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[2] - # elseif t <= 5 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[1] - # elseif t <= 6 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[2] - # elseif t <= 7 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[1] - # elseif t <= 8 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[2] - # elseif t <= 9 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[1] - # elseif t <= 10 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[2] - # elseif t <= 11 - # cmd.motor = chicane_speed - # cmd.servo = chicane_turn[1] - # ************************************** - # CONTINUOUS ACCELERATION: - # elseif t <= 123 - # cmd.motor = 0.0+(t-3)/20 - # cmd.servo = 0.0#-(t-3.0)/300-0.15 - # elseif t <= 8 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = 100.0 - # elseif t <= 13 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = 105.0 - # elseif t <= 18 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = 110.0 - # elseif t <= 23 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = 115.0 - # elseif t <= 28 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 97.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = 120.0 - # elseif t <= 33 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.4 - # elseif t <= 38 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.45 - # elseif t <= 43 # CHECK TIME AND ACCELERATION !!! - # cmd.motor = 1.0 # CHECK TIME AND ACCELERATION !!! - # cmd.servo = -0.5 - # elseif t <= 44 - # cmd.motor = -2.0 - # cmd.servo = 0 - else - cmd.motor = 70.0 - cmd.servo = 90.0 + + + + + if t <= t_next + + cmd.motor = cmd_m + + #t_cmd[index] = t + + elseif t > t_next && t <= t_next + 6 + + cmd.motor = 80 + + #t_no_cmd[index] = t + + + # else + # t_next = t + 4 + + # println("t== ",t) + # println("time_index== ",time_index) + + # time_vector = time_index+0.1:.02:t-0.1 + + # v = zeros(length(time_vector)) + # command = zeros(length(time_vector)) + + # println("v= ",v) + + # # println("time vector= ",time_vector) + + # for i=1:length(time_vector) + + # v[i] = vel_est_log.z[time_vector[i].>vel_est_log.t,1][end] + # command[i] = cmd_pwm_log.z[time_vector[i].>cmd_pwm_log.t,1][end] + # end + + # println("v= ",v) + + # v_opt = v[1:end] + # command_opt = command[1:end] + # v_opt = v[command.>90] + # command_opt = command[command.>90] + + # figure() + # plot(v_opt) + + # # println("FLAG=",v_opt) + + # a[index] = v_opt[end]/4 + + # println("a= ",a[index]) + + + + # index=index+1 + + # time_index = t end + cmd.header.stamp = get_rostime() publish(pub, cmd) rossleep(loop_rate) end - # Clean up buffers + # Clean up buffers clean_up(gps_meas) clean_up(imu_meas) clean_up(cmd_log) @@ -172,10 +345,17 @@ function main() log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4])-2.jld" warn("Warning: File already exists.") end - save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) + save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log)#,"t_cmd",t_cmd,"t_no_cmd",t_no_cmd) println("Exiting node... Saved recorded data to $log_path.") end if ! isinteractive() main() end + + + + + + + diff --git a/workspace/src/barc/src/open_loop_francesco.jl b/workspace/src/barc/src/open_loop_francesco.jl new file mode 100644 index 00000000..54c6fc73 --- /dev/null +++ b/workspace/src/barc/src/open_loop_francesco.jl @@ -0,0 +1,117 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Vel_est +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +@rosimport marvelmind_nav.msg: hedge_pos +rostypegen() +using barc.msg +using geometry_msgs.msg +using sensor_msgs.msg +using std_msgs.msg +using marvelmind_nav.msg +using JLD +include("barc_lib/log_functions.jl") + +function main() + + # Set up logging + buffersize = 60000 + gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,9)) + cmd_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_pwm_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + vel_est_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,5)) + pos_info_log = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,20)) + + # Initialize ROS node and topics + init_node("mpc_traj") + s1 = Subscriber("ecu", ECU, ECU_callback, (cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + s2 = Subscriber("imu/data", Imu, IMU_callback, (imu_meas,), queue_size=1)::RobotOS.Subscriber{sensor_msgs.msg.Imu} + s3 = Subscriber("ecu_pwm", ECU, ECU_PWM_callback, (cmd_pwm_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + s4 = Subscriber("hedge_pos", hedge_pos, GPS_callback, (gps_meas,), queue_size=1)::RobotOS.Subscriber{marvelmind_nav.msg.hedge_pos} + s5 = Subscriber("pos_info", pos_info, pos_info_callback, (pos_info_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s6 = Subscriber("vel_est", Vel_est, vel_est_callback, (vel_est_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.Vel_est} + + run_id = get_param("run_id") + + loop_rate = Rate(10) + pub = Publisher("ecu_pwm", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + cmd = ECU() # command type + cmd.motor = 0 + cmd.servo = 0 + cmd.header.stamp = get_rostime() + println("Starting open loop control. Waiting.") + rossleep(3.0) + + t0_ros = get_rostime() + t0 = to_sec(t0_ros) + t = 0.0 + + t_next = 4 + + gps_meas.i = 1 + imu_meas.i = 1 + cmd_log.i = 1 + cmd_pwm_log.i = 1 + vel_est_log.i = 1 + pos_info_log.i = 1 + + cmd_m = 94 + + + while ! is_shutdown() # run exactly x seconds + t = to_sec(get_rostime())-t0 + + println(to_sec(get_rostime())) + + t1 = 0 + t2 = 0 + + if t <= t_next + + cmd.motor = cmd_m + + elseif t > t_next && t <= t_next + 4 + + cmd.motor = 90 + + else + t_next = t + 4 + end + + cmd.header.stamp = get_rostime() + publish(pub, cmd) + + rossleep(loop_rate) + end + + # Clean up buffers + clean_up(gps_meas) + clean_up(imu_meas) + clean_up(cmd_log) + clean_up(cmd_pwm_log) + clean_up(pos_info_log) + clean_up(vel_est_log) + + println("Exiting open loop control.") + log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4]).jld" + if isfile(log_path) + log_path = "$(homedir())/open_loop/output-record-$(run_id[1:4])-2.jld" + warn("Warning: File already exists.") + end + save(log_path,"gps_meas",gps_meas,"imu_meas",imu_meas,"cmd_log",cmd_log,"cmd_pwm_log",cmd_pwm_log,"pos_info",pos_info_log,"vel_est",vel_est_log) + println("Exiting node... Saved recorded data to $log_path.") +end + +if ! isinteractive() + main() +end + + + + + + + From fd011ace568f33cf06a47922f014d6f508668f57 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 22 Jan 2018 15:06:34 -0800 Subject: [PATCH 135/183] modified track --- eval_sim/eval_data.jl | 6 +++--- workspace/src/barc/src/Localization_helpers.py | 6 +++--- workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index aad28488..f8cc5188 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1400,9 +1400,9 @@ function create_track(w) add_curve(theta,40,-pi/2) add_curve(theta,10,0) add_curve(theta,40,-pi/2) - add_curve(theta,20,pi/10) - add_curve(theta,30,-pi/5) - add_curve(theta,20,pi/10) + add_curve(theta,20,-pi/10) + add_curve(theta,30,pi/5) + add_curve(theta,20,-pi/10) add_curve(theta,40,-pi/2) add_curve(theta,10,0) add_curve(theta,40,-pi/2) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index c46092cc..dd767dc2 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -168,9 +168,9 @@ def create_track(self): theta = add_curve(theta,80,-pi/2) theta = add_curve(theta,20,0) theta = add_curve(theta,80,-pi/2) - theta = add_curve(theta,40,pi/10) - theta = add_curve(theta,60,-pi/5) - theta = add_curve(theta,40,pi/10) + theta = add_curve(theta,40,-pi/10) + theta = add_curve(theta,60,pi/5) + theta = add_curve(theta,40,-pi/10) theta = add_curve(theta,80,-pi/2) theta = add_curve(theta,20,0) theta = add_curve(theta,80,-pi/2) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 5afeaf64..11b6a209 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -457,7 +457,7 @@ type MpcModel_convhull z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_lb_6s = ones(mpcParams.N,1) * [0.5 -0.3]#[-1.0 -0.3] # lower bounds on steering u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds for i=1:2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 5420734d..7702f827 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,14 +47,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 16 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 8 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 3 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 11 + mpcParams.N = 16 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) @@ -66,7 +66,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 10 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = [20.0,10.0,10.0,30.0,80.0,50.0]#20.0*[2.0,2.0,1.0,2.0,5.0,5.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 20.0*[2.0,2.0,1.0,5.0,5.0,5.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories From 564e0313c7c1f1cfffa06218a6bf071f8d82c2c3 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 22 Jan 2018 17:30:53 -0800 Subject: [PATCH 136/183] changed settings for mapping tests --- workspace/src/barc/src/open_loop.jl | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 05839014..4cd7765e 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -250,7 +250,9 @@ function main() vel_est_log.i = 1 pos_info_log.i = 1 - cmd_m = 102 + cmd_m = 105 + + cmd_s = 93 # t_cmd = zeros(10) # t_no_cmd = zeros(10) @@ -271,6 +273,7 @@ function main() if t <= t_next cmd.motor = cmd_m + cmd.servo = cmd_s #t_cmd[index] = t From 3d1732f23c824aa9ac2daa34cddb9760ea095c76 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 23 Jan 2018 10:43:55 -0800 Subject: [PATCH 137/183] small changes for mapping --- eval_sim/test_load.jl | 4 +++- .../state_estimation_SensorKinematicModel.py | 19 +++++++++++++++++-- 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/eval_sim/test_load.jl b/eval_sim/test_load.jl index 5487c012..60f79c5f 100644 --- a/eval_sim/test_load.jl +++ b/eval_sim/test_load.jl @@ -79,9 +79,11 @@ function test(path::AbstractString) x = inv(A'*A)*A'*B + println("the computed constants are: ",x) + println("****quick check of the results****") - a_test = x[1]*commands[1] + x[2] - x[3]*v_final[1] + a_test = x[1]*commands[1] + x[2] - x[3]*v_final[end] println("a_test= ",a_test) diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 1eaa33c7..701d7380 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -50,6 +50,8 @@ class StateEst(object): # Velocity vel_meas = 0.0 vel_updated = False + vel_prev = 0.0 + vel_count = 0.0 # GPS x_meas = 0.0 @@ -169,8 +171,21 @@ def imu_callback(self, data): self.imu_updated = True def encoder_vel_callback(self, data): - self.vel_meas = data.vel_est - self.vel_updated = True + + if data.vel_est != self.vel_prev: + self.vel_meas = data.vel_est + self.vel_updated = True + self.vel_prev = data.vel_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True + + + # self.vel_meas = data.vel_est + # self.vel_updated = True # state estimation node def state_estimation(): From bef7bb00b1576f46486aa5bf7726e4cc4f8066d7 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Wed, 24 Jan 2018 11:12:58 -0800 Subject: [PATCH 138/183] added script for deceleration mapping --- eval_sim/test_load.jl | 172 ++++++++++++++++++++++++---- workspace/src/barc/src/open_loop.jl | 13 ++- 2 files changed, 156 insertions(+), 29 deletions(-) diff --git a/eval_sim/test_load.jl b/eval_sim/test_load.jl index 60f79c5f..981749b6 100644 --- a/eval_sim/test_load.jl +++ b/eval_sim/test_load.jl @@ -1,22 +1,27 @@ using PyPlot using JLD -function test(path::AbstractString) +function test(path_acceleration::AbstractString,path_deceleration::AbstractString) - files = readdir(path) + files_acc = readdir(path_acceleration) + files_dec = readdir(path_deceleration) - a = zeros(length(files)) + a = zeros(length(files_acc) + length(files_dec)) + println("number of tests= ",length(a)) - v_final = zeros(length(files)) + ## Loop for acceleration - commands = zeros(length(files)) + v_acceleration1 = [] #zeros(length(files)) + v_acceleration2 = [] - for index = 1:length(files) + commands_acc = [] #zeros(length(files)) - file = files[index] + for index = 1:length(files_acc) - log_path = "$(homedir())/open_loop/$file" + file = files_acc[index] + + log_path = "$(homedir())/open_loop/acceleration/$file" d_rec=load(log_path) @@ -45,47 +50,166 @@ function test(path::AbstractString) figure(1) plot(v_opt) + grid("on") + # println("length= ",length(files)) - # println("v_opt= ",v_opt) + # println("v_opt= ",length(v_opt)) + # println("cmd_opt= ",length(cmd_opt)) # println("v_opt[end]= ",v_opt[end]) # println("a[i]= ",a[i]) # println("v_opt[end]/4= ",v_opt[end]/4) - v_final[index] = v_opt[end] + v_lms1 = zeros(length(v_opt)-1) + v_lms2 = zeros(length(v_opt)-1) + + + for j = 1:length(v_opt)-1 + + v_lms1[j] = v_opt[j+1] - v_opt[j] + v_lms2[j] = v_opt[j] + + end + + v_acceleration1 = vcat(v_acceleration1,v_lms1) + v_acceleration2 = vcat(v_acceleration2,v_lms2) a[index] = v_opt[end]/4 - commands[index]=cmd_opt[end] + commands_acc = vcat(commands_acc,cmd_opt[1:end-1]) - println("a= ",a) - println("number of tests= ",length(a)) + # println("a= ",a) + figure(2) plot(cmd_opt[1],a[index], "*") + grid("on") + + end + + ## Loop for deceleration + + v_deceleration1 = [] #zeros(length(files)) + v_deceleration2 = [] + + commands_dec = [] #zeros(length(files)) + + for index = 1:length(files_dec) + + file = files_dec[index] + + log_path = "$(homedir())/open_loop/deceleration/$file" + + d_rec=load(log_path) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.<100] + cmd_opt = cmd[cmd.<100] + + v_equal = 0 + flag = 0 + + for i = 2:length(v_opt) + + if v_opt[i]==v_opt[i-1] + v_equal = v_equal + 1 + else + v_equal = 0 + end + + if v_equal > 10 + flag = i + break + end + end + + v_opt = v_opt[1:flag] + cmd_opt = cmd_opt[1:flag] + + a[length(files_acc) + index] = (- v_opt[1])/4 + + # plot(cmd_opt[5],a[length(files_acc) + index], "*") + # grid("on") + + + + figure(3) + + plot(v_opt) + grid("on") + + + v_lms1 = zeros(length(v_opt)-1) + v_lms2 = zeros(length(v_opt)-1) + + + for j = 1:length(v_opt)-1 + + v_lms1[j] = v_opt[j+1] - v_opt[j] + v_lms2[j] = v_opt[j] + + end + + v_deceleration1 = vcat(v_deceleration1,v_lms1) + v_deceleration2 = vcat(v_deceleration2,v_lms2) + commands_dec = vcat(commands_dec,cmd_opt[1:end-1]) + + + end + + + + # Least Mean Square - A = ones(length(files),3) - B = zeros(length(files),1) - x = zeros(3,1) #[c1_+;c2_+,cM] + # println("length of v_acceleration1= ",length(v_acceleration1)) + # println("length of commands= ",length(commands)) - A[:,1] = commands - A[:,3] = -v_final - B = a + A = zeros(length(v_acceleration1)+length(v_deceleration1),5) + B = zeros(length(v_acceleration1)+length(v_deceleration1),1) + x = zeros(5,1) #[c1_+;c2_+,cM] - x = inv(A'*A)*A'*B + A[1:length(commands_acc),1] = commands_acc + A[1:length(commands_acc),2] = 1 + A[1:length(v_acceleration2),3] = -v_acceleration2 + A[length(v_acceleration2)+1:end,3] = -v_deceleration2 + A[length(commands_acc)+1:end,4] = commands_dec + A[length(commands_acc)+1:end,5] = 1 + + B[1:length(v_acceleration1)] = v_acceleration1 + B[length(v_acceleration1)+1:end] = v_deceleration1 - println("the computed constants are: ",x) + A = 0.02*A # 0.02 is the discretization - println("****quick check of the results****") - a_test = x[1]*commands[1] + x[2] - x[3]*v_final[end] + x = inv(A'*A)*A'*B + + println("the computed constants are: ", x) - println("a_test= ",a_test) + # x_axis = (findmin(commands)[1]):0.02:(findmax(commands)[1]) + # y_axis = x[1]*x_axis + x[2] + # plot(x_axis,y_axis) end diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 4cd7765e..533faf69 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -241,7 +241,7 @@ function main() t0 = to_sec(t0_ros) t = 0.0 - t_next = 4 + t_next = 2 gps_meas.i = 1 imu_meas.i = 1 @@ -250,7 +250,7 @@ function main() vel_est_log.i = 1 pos_info_log.i = 1 - cmd_m = 105 + cmd_m = 100 cmd_s = 93 @@ -277,14 +277,17 @@ function main() #t_cmd[index] = t - elseif t > t_next && t <= t_next + 6 + elseif t > t_next && t <= t_next + 4 - cmd.motor = 80 + cmd.motor = 40 #t_no_cmd[index] = t - # else + else + cmd.motor = -1 + + println("******FLAG*******") # t_next = t + 4 # println("t== ",t) From 168db483b30bf5e7f49b4abcfd51c5a4c80e3f6e Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 25 Jan 2018 14:14:50 -0800 Subject: [PATCH 139/183] added plots for debugging --- eval_sim/eval_data.jl | 104 +++++++++++++----- workspace/src/barc/src/LMPC_node.jl | 71 ++---------- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 14 +-- .../src/barc/src/controller_low_level.py | 9 +- 5 files changed, 98 insertions(+), 102 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index f8cc5188..402dc610 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -108,6 +108,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) selStates = Data["selStates"] statesCost = Data["statesCost"] pred_sol = Data["pred_sol"] + pred_input = Data["pred_input"] one_step_error = Data["one_step_error"] lapStatus = Data["lapStatus"] posInfo = Data["posInfo"] @@ -119,6 +120,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cost = Data["mpcCost"] costSlack = Data["mpcCostSlack"] obs_log = Data["obs_log"] + sol_u = Data["sol_u"] #status = Data["status"] Nl = selectedStates.Nl @@ -131,6 +133,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) track = create_track(0.4) + println("prediction horizon N= ", size(pred_sol)[1]) @@ -138,7 +141,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) for i = laps pred_sol_xy = xyObstacle(oldSS,obs_log,1,i,track) - println("pred sol= ",pred_sol_xy[:,1]) + #println("pred sol= ",pred_sol_xy[:,1]) # for index=1:buffersize # if status[index,i] == :UserLimit # flag[1]=index @@ -168,10 +171,20 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cpsi3 = cpsi[1:currentIt,3,i] figure(1) - plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") - plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - grid("on") + + #for i=1:4:size(x_est,1) + for index=1:length(oldSS_xy[:,1,i]) + z_pred = zeros(size(pred_sol)[1],4) + #z_pred[1,:] = x_est[i,:] + z_pred[1,:] = oldSS_xy[index,:,i] + for j=2:size(pred_sol)[1] + z_pred[j,:] = simModel(z_pred[j-1,:],pred_input[j-1,:,index,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-+") + end # ellfig = figure(1) # ax = ellfig[:add_subplot](1,1,1) @@ -194,7 +207,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) t = linspace(1,currentIt,currentIt) - figure(2) + figure() subplot(221) plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") @@ -226,7 +239,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) grid("on") - figure(3) + figure() subplot(221) plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") @@ -244,7 +257,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) - figure(4) + figure() subplot(221) plot(t,oldSS.oldSS[1:currentIt,1,i],"-*") @@ -270,7 +283,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("ePsi in lap $i") grid("on") - figure(5) + figure() subplot(221) plot(t,oldSS.oldSS[1:currentIt,5,i],"-*") #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) @@ -289,10 +302,9 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("Overall velocity in lap $i") grid("on") - println("average velocity= ",mean(velocity)) - figure(6) + figure() subplot(221) plot(t,cvx1,t,cvx2,t,cvx3) @@ -312,19 +324,23 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("C_Psi in lap $i") grid("on") - figure(7) + figure() plot(t,cost[1:currentIt,2,i],t,cost[1:currentIt,3,i],t,cost[1:currentIt,4,i],t,cost[1:currentIt,6,i]) legend(["terminal Cost","control Cost","derivative Cost","lane Cost"]) title("Costs of the Mpc") grid("on") - figure(8) + figure() plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i]) legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) title("Slack costs") grid("on") + + + + if switch == true @@ -361,10 +377,10 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) #olds3 = selStates[2*Np+1:3*Np,6,j,i] - + t = linspace(1,j,j) - figure(9) + figure(15) clf() subplot(221) plot(s_pred,vx_pred,"or") @@ -403,7 +419,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) grid("on") - figure(10) + figure(16) clf() subplot(221) plot(s_pred,eY_pred,"or") @@ -414,10 +430,39 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("State eY in lap $i, iteration $j ") grid("on") + figure(17) + clf() + + subplot(121) + velocity= sqrt(oldSS.oldSS[1:j,2,i].^2 + oldSS.oldSS[1:j,1,i].^2) + plot(t,velocity,"-*") + legend(["velocity"]) + + subplot(122) + plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + legend(["a_x","d_f"]) + + title("Comparison between velocity and inputs in lap $i, iteration $j ") + grid("on") + + figure(18) + clf() + + subplot(121) + plot(t,oldSS.oldSS[1:j,5,i],"-*") + legend(["e_y"]) + + subplot(122) + plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + legend(["a_x","d_f"]) + + title("Comparison between e_y and inputs in lap $i, iteration $j ") + grid("on") + - sleep(3) + sleep(5) end end end @@ -707,6 +752,7 @@ function eval_LMPC(code::AbstractString) t0 = t[1] + figure(2) ax1=subplot(311) plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*") @@ -781,21 +827,21 @@ function eval_LMPC(code::AbstractString) grid(1) # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA: # CURRENT HEADING (PLOTTED BY A LINE) - for i=1:10:size(pos_info.t,1) - dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] - lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] - plot(lin[:,1],lin[:,2],"-+") - end + # for i=1:10:size(pos_info.t,1) + # dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + # lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + # plot(lin[:,1],lin[:,2],"-+") + # end # PREDICTED PATH - # for i=1:4:size(x_est,1) - # z_pred = zeros(11,4) - # z_pred[1,:] = x_est[i,:] - # for j=2:11 - # z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) - # end - # plot(z_pred[:,1],z_pred[:,2],"-*") - # end + for i=1:4:size(x_est,1) + z_pred = zeros(11,4) + z_pred[1,:] = x_est[i,:] + for j=2:11 + z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-*") + end # PREDICTED REFERENCE PATH (DEFINED BY POLYNOM) # for i=1:size(x_est,1) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index d5372406..f709d966 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -131,6 +131,7 @@ function main() selStates_log = zeros(selectedStates.Nl*selectedStates.Np,6,buffersize,30) #array to log the selected states in every iteration of every lap statesCost_log = zeros(selectedStates.Nl*selectedStates.Np,buffersize,30) #array to log the selected states' costs in every iteration of every lap log_predicted_sol = zeros(mpcParams.N+1,7,buffersize,30) + log_predicted_input = zeros(mpcParams.N,2,buffersize,30) log_onestep = zeros(buffersize,6,30) log_epsalpha = zeros(6,buffersize,30) log_cvx = zeros(buffersize,3,30) @@ -174,11 +175,6 @@ function main() mpcSol.u = zeros(10,2) mpcSol.a_x = 0 mpcSol.d_f = 0 - - #mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724] - #mpcCoeff.c_Psi = [-0.3747957571478858,-0.005013036784512181,5.068342163488241] - #mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765] - #mpcCoeff.c_Vy = [0.002968102163011754,-0.09886540158694888,0.012234790760745129,1.099308717654053] # Precompile coeffConstraintCost: oldTraj.oldTraj[1:buffersize,6,1] = linspace(0,posInfo.s_target,buffersize) @@ -396,50 +392,6 @@ function main() mpcParams.Q_obs = ones(selectedStates.Nl*selectedStates.Np) - ############################################################## - # #SSet warm start - # if lapStatus.currentLap > n_pf && lapStatus.currentIt > 1 - # if selectedStates.version == false && obstacle.obstacle_active == false - - # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) - # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) - # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) - # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) - # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) - # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) - # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) - # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) - # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) - # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) - # setvalue(mdl_convhull.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) - # setvalue(mdl_convhull.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) - - # #setvalue(mdl_convhull.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - # setvalue(mdl_convhull.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) - - # elseif selectedStates.version == false && obstacle.obstacle_active == true - - # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]) - # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]) - # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,2],mpcSol.z[2:mpcParams.N+1,2]) - # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,2],mpcSol.z[mpcParams.N+1,2]) - # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,3],mpcSol.z[2:mpcParams.N+1,3]) - # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,3],mpcSol.z[mpcParams.N+1,3]) - # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,4],mpcSol.z[2:mpcParams.N+1,4]) - # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,4],mpcSol.z[mpcParams.N+1,4]) - # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,5],mpcSol.z[2:mpcParams.N+1,5]) - # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,5],mpcSol.z[mpcParams.N+1,5]) - # setvalue(mdl_obstacle.z_Ol[1:mpcParams.N,6],mpcSol.z[2:mpcParams.N+1,6]) - # setvalue(mdl_obstacle.z_Ol[mpcParams.N+1,6],mpcSol.z[mpcParams.N+1,6]) - - # #setvalue(mdl_obstacle.z_Ol[:,6],mpcSol.z[:,6]-posInfo.s_target) - # setvalue(mdl_obstacle.alpha[:],(1/(selectedStates.Nl*selectedStates.Np))*ones(selectedStates.Nl*selectedStates.Np)) - # end - # end - ############################################################## - - - if lapStatus.currentLap > 1 if lapStatus.currentIt == (oldSS.postbuff+2) oldSS.oldSS[oldSS.oldCost[lapStatus.currentLap-1]:oldSS.oldCost[lapStatus.currentLap-1]+oldSS.postbuff+1,1:5,lapStatus.currentLap-1] = zCurr[1:oldSS.postbuff+2,1:5] @@ -453,28 +405,19 @@ function main() obs_temp = obs_curr[lapStatus.currentIt,:,:] - - if posInfo.s_target-posInfo.s < obstacle.obs_detect # meaning that I could possibly detect obstacles after the finish line index1=find(obs_curr[lapStatus.currentIt,1,:].< obstacle.obs_detect+posInfo.s-posInfo.s_target) # look for obstacles that could cause problems obs_temp[1,1,index1] = posInfo.s_target + obs_curr[lapStatus.currentIt,1,index1] - end - dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) - # println("dist= ",dist) - # println("obstacle's position= ",obs_temp[1,1,:]) obs_near = obs_temp[1,:,index] end - - - # Find coefficients for cost and constraints if lapStatus.currentLap > n_pf tic() @@ -484,7 +427,8 @@ function main() end #println("Starting solving.") - # Solve the MPC problem + + ### Solve the MPC problem tic() if lapStatus.currentLap <= n_pf z_pf = [zCurr[i,6],zCurr[i,5],zCurr[i,4],norm(zCurr[i,1:2]),acc0] # use kinematic model and its states @@ -561,9 +505,9 @@ function main() log_input[lapStatus.currentIt,:,lapStatus.currentLap] = uCurr[i,:] log_mpcCost[lapStatus.currentIt,:,lapStatus.currentLap] = mpcSol.cost log_obs[lapStatus.currentIt,:,:,lapStatus.currentLap] = obs_curr[i,:,:] - if lapStatus.currentLap > n_pf - log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack - end + # if lapStatus.currentLap > n_pf + # log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack + # end #log_status[lapStatus.currentIt,lapStatus.currentLap] = mpcSol.solverStatus @@ -589,6 +533,7 @@ function main() if lapStatus.currentLap > n_pf log_predicted_sol[:,:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.z + log_predicted_input[:,:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.u #log_epsalpha[:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.eps_alpha end @@ -611,7 +556,7 @@ function main() "x_est",log_state_x[1:k,:],"coeffX",log_coeffX[1:k,:],"coeffY",log_coeffY[1:k,:],"c_Vx",log_c_Vx[1:k,:], "c_Vy",log_c_Vy[1:k,:],"c_Psi",log_c_Psi[1:k,:],"cmd",log_cmd[1:k,:],"step_diff",log_step_diff[1:k,:], "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k],"selectedStates",selectedStates,"one_step_error",log_onestep, - "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"lapStatus",lapStatus, + "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"pred_input",log_predicted_input,"lapStatus",lapStatus, "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy,"input",log_input, "mpcCost",log_mpcCost,"mpcCostSlack",log_mpcCostSlack,"obs_log",log_obs) #,"status",log_status) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 11b6a209..5afeaf64 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -457,7 +457,7 @@ type MpcModel_convhull z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [0.5 -0.3]#[-1.0 -0.3] # lower bounds on steering + u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds for i=1:2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 7702f827..02ef97ed 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,14 +47,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 8 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 6 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 3 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) - selectedStates.version = false + selectedStates.version = true - mpcParams.N = 16 + mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) @@ -64,9 +64,9 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 10 # weight on the soft constraint for the lane + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 20.0*[2.0,2.0,1.0,5.0,5.0,5.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = [5.0,5.0,5.0,5.0,5.0,5.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories @@ -76,7 +76,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1*[10,10] # cost matrix for derivative cost of inputs - mpcParams_pF.vPathFollowing = 0.9 # reference speed for first lap of path following + mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay @@ -91,7 +91,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track modelParams.I_z = 0.03 modelParams.c_f = 0.5 # friction coefficient: xDot = - c_f*xDot (aerodynamic+tire) - posInfo.s_target = 5.0 + posInfo.s_target = 19.11 oldTraj.oldTraj = NaN*ones(buffersize,7,30) oldTraj.oldInput = zeros(buffersize,2,30) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 2b8e2107..b1a78aa3 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -50,10 +50,15 @@ def pwm_converter_callback(self, msg): self.motor_pwm = 90.0 elif FxR > 0: #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino - self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + #self.motor_pwm = 90.74 + 6.17*FxR + self.motor_pwm = max(94,90.12 + 5.24*FxR) + #self.motor_pwm = 90.12 + 5.24*FxR # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down - self.motor_pwm = 93.5 + 46.73*FxR + #self.motor_pwm = 93.5 + 46.73*FxR + #self.motor_pwm = 98.65 + 67.11*FxR + self.motor = 69.95 + 68.49*FxR self.update_arduino() def neutralize(self): self.motor_pwm = 40 # slow down first From d9097d38b9fc2da8339273dcb6318b2504011537 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 25 Jan 2018 15:49:20 -0800 Subject: [PATCH 140/183] minor changes in plots --- eval_sim/eval_data.jl | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 402dc610..0b416f13 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -359,22 +359,22 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) oldvx = selStates[1:Np,1,j,i] oldvx2 = selStates[Np+1:2*Np,1,j,i] - #oldvx3 = selStates[2*Np+1:3*Np,1,j,i] + oldvx3 = selStates[2*Np+1:3*Np,1,j,i] oldvy = selStates[1:Np,2,j,i] oldvy2 = selStates[Np+1:2*Np,2,j,i] - #oldvy3 = selStates[2*Np+1:3*Np,2,j,i] + oldvy3 = selStates[2*Np+1:3*Np,2,j,i] oldpsiDot = selStates[1:Np,3,j,i] oldpsiDot2 = selStates[Np+1:2*Np,3,j,i] - #oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] + oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] oldePsi = selStates[1:Np,4,j,i] oldePsi2 = selStates[Np+1:2*Np,4,j,i] - #oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] + oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] oldeY = selStates[1:Np,5,j,i] oldeY2 = selStates[Np+1:2*Np,5,j,i] - #oldeY3 = selStates[2*Np+1:3*Np,5,j,i] + oldeY3 = selStates[2*Np+1:3*Np,5,j,i] olds = selStates[1:Np,6,j,i] olds2 = selStates[Np+1:2*Np,6,j,i] - #olds3 = selStates[2*Np+1:3*Np,6,j,i] + olds3 = selStates[2*Np+1:3*Np,6,j,i] t = linspace(1,j,j) @@ -386,7 +386,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,vx_pred,"or") plot(olds,oldvx,"b") plot(olds2,oldvx2,"b") - #plot(olds3,oldvx3,"b") + plot(olds3,oldvx3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) title("State vx in lap $i, iteration $j") grid("on") @@ -395,7 +395,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,vy_pred,"or") plot(olds,oldvy,"b") plot(olds2,oldvy2,"b") - #plot(olds3,oldvy3,"b") + plot(olds3,oldvy3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) title("State vy in lap $i, iteration $j ") grid("on") @@ -404,7 +404,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,psiDot_pred,"or") plot(olds,oldpsiDot,"b") plot(olds2,oldpsiDot2,"b") - #plot(olds3,oldpsiDot3,"b") + plot(olds3,oldpsiDot3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) title("State psiDot in lap $i , iteration $j") grid("on") @@ -413,7 +413,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,ePsi_pred,"or") plot(olds,oldePsi,"b") plot(olds2,oldePsi2,"b") - #plot(olds3,oldePsi3,"b") + plot(olds3,oldePsi3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) title("State ePsi in lap $i, iteration $j ") grid("on") @@ -425,7 +425,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,eY_pred,"or") plot(olds,oldeY,"b") plot(olds2,oldeY2,"b") - #plot(olds3,oldeY3,"b") + plot(olds3,oldeY3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) title("State eY in lap $i, iteration $j ") grid("on") @@ -780,14 +780,17 @@ function eval_LMPC(code::AbstractString) subplot(311) title("c_Vx") plot(t-t0,c_Vx) + legend(["1","2","3"]) grid("on") subplot(312) title("c_Vy") plot(t-t0,c_Vy) + legend(["1","2","3","4"]) grid("on") subplot(313) title("c_Psi") plot(t-t0,c_Psi) + legend(["1","2","3"]) grid("on") # *********** CURVATURE ********************* From 30e11bede79dc2b98c75b34a9702fad98cb218b4 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Fri, 26 Jan 2018 15:11:52 -0800 Subject: [PATCH 141/183] new plots added --- eval_sim/eval_data.jl | 144 +++++++++++------- eval_sim/test_load.jl | 2 + workspace/src/barc/src/LMPC_node.jl | 8 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 10 +- 5 files changed, 105 insertions(+), 61 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 0b416f13..b74ebd6b 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -131,7 +131,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) flag = zeros(2) - track = create_track(0.4) + track = create_track(0.3) println("prediction horizon N= ", size(pred_sol)[1]) @@ -175,16 +175,16 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - #for i=1:4:size(x_est,1) - for index=1:length(oldSS_xy[:,1,i]) - z_pred = zeros(size(pred_sol)[1],4) - #z_pred[1,:] = x_est[i,:] - z_pred[1,:] = oldSS_xy[index,:,i] - for j=2:size(pred_sol)[1] - z_pred[j,:] = simModel(z_pred[j-1,:],pred_input[j-1,:,index,i],0.1,0.125,0.125) - end - plot(z_pred[:,1],z_pred[:,2],"-+") - end + + # for index=1:length(oldSS_xy[:,1,i]) + # z_pred = zeros(size(pred_sol)[1],4) + # #z_pred[1,:] = x_est[i,:] + # z_pred[1,:] = oldSS_xy[index,:,i] + # for j=2:size(pred_sol)[1] + # z_pred[j,:] = simModel(z_pred[j-1,:],pred_input[j-1,:,index,i],0.1,0.125,0.125) + # end + # plot(z_pred[:,1],z_pred[:,2],"-+") + # end # ellfig = figure(1) # ax = ellfig[:add_subplot](1,1,1) @@ -203,58 +203,67 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) grid("on") title("X-Y view of Lap $i") + t = linspace(1,currentIt,currentIt) - t = linspace(1,currentIt,currentIt) - figure() - subplot(221) - plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - # annotate("UserLimit",xy=[flag[1],one_step_error[flag[1],1,flag[2]]],xytext=[flag[1]+0.1,one_step_error[flag[1],1,flag[2]]+0.1],xycoords="data",arrowprops=["facecolor"=>"black"]) - title("One step prediction error for v_x in lap $i") - #ylim(-0.0001,findmax(one_step_error[1:currentIt,1,i])[1]) - legend(["ospe","a_x","d_f"]) - grid("on") - subplot(222) - plot(t,one_step_error[1:currentIt,2,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - #ylim(-0.0001,findmax(one_step_error[1:currentIt,2,i])[1]) - legend(["ospe","a_x","d_f"]) - title("One step prediction error for v_y in lap $i") - grid("on") + # figure() - subplot(223) - plot(t,one_step_error[1:currentIt,3,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - #ylim(-0.0001,findmax(one_step_error[1:currentIt,3,i])[1]) - legend(["ospe","a_x","d_f"]) - title("One step prediction error for psiDot in lap $i") - grid("on") + # subplot(221) + # plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # # annotate("UserLimit",xy=[flag[1],one_step_error[flag[1],1,flag[2]]],xytext=[flag[1]+0.1,one_step_error[flag[1],1,flag[2]]+0.1],xycoords="data",arrowprops=["facecolor"=>"black"]) + # title("One step prediction error for v_x in lap $i") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,1,i])[1]) + # legend(["ospe","a_x","d_f"]) + # grid("on") - subplot(224) - plot(t,one_step_error[1:currentIt,4,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - #ylim(-0.0001,findmax(one_step_error[1:currentIt,4,i])[1]) - legend(["ospe","a_x","d_f"]) - title("One step prediction error for ePsi in lap $i") - grid("on") + # subplot(222) + # plot(t,one_step_error[1:currentIt,2,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,2,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for v_y in lap $i") + # grid("on") + # subplot(223) + # plot(t,one_step_error[1:currentIt,3,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,3,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for psiDot in lap $i") + # grid("on") - figure() + # subplot(224) + # plot(t,one_step_error[1:currentIt,4,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,4,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for ePsi in lap $i") + # grid("on") - subplot(221) - plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - #ylim(-0.0001,findmax(one_step_error[1:currentIt,5,i])[1]) - legend(["ospe","a_x","d_f"]) - title("One step prediction error for eY in lap $i") - grid("on") - subplot(222) - plot(t,one_step_error[1:currentIt,6,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) - legend(["ospe","a_x","d_f"]) - title("One step prediction error for s in lap $i") + # figure() + + # subplot(221) + # plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,5,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for eY in lap $i") + # grid("on") + + # subplot(222) + # plot(t,one_step_error[1:currentIt,6,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for s in lap $i") + # grid("on") + + figure() + plot(t,one_step_error[1:currentIt,1,i],t,one_step_error[1:currentIt,2,i],t,one_step_error[1:currentIt,3,i],t,one_step_error[1:currentIt,4,i],t,one_step_error[1:currentIt,5,i],t,one_step_error[1:currentIt,6,i]) + legend(["vx","vy","psiDot","ePsi","eY","s"]) grid("on") + title("One step prediction errors") + # println("one step prediction error= ",one_step_error[1:30,:,i]) figure() @@ -378,6 +387,25 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) t = linspace(1,j,j) + + figure(19) + clf() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + #for i=1:4:size(x_est,1) + # for index=1:length(oldSS_xy[:,1,i]) + z_pred = zeros(size(pred_sol)[1],4) + #z_pred[1,:] = x_est[i,:] + z_pred[1,:] = oldSS_xy[j,:,i] + for j2=2:size(pred_sol)[1] + z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-+") + grid("on") + title("Predicted solution in lap $i, iteration $j") + # end figure(15) @@ -459,10 +487,24 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) title("Comparison between e_y and inputs in lap $i, iteration $j ") grid("on") + figure(20) + clf() + subplot(211) + plot(t,oldSS.oldSS[1:50,5,i],"-*") + title("eY in lap $i, iteration $j") + grid("on") + plot(linspace(j,j+size(pred_sol)[1],size(pred_sol)[1]),pred_sol[:,5,j,i],"-+") + + subplot(212) + plot(t,cost[1:j,6,i]) + title("lane cost in lap $i, iteration $j") + grid("on") + + - sleep(5) + sleep(5) end end end diff --git a/eval_sim/test_load.jl b/eval_sim/test_load.jl index 981749b6..3a21b483 100644 --- a/eval_sim/test_load.jl +++ b/eval_sim/test_load.jl @@ -211,5 +211,7 @@ function test(path_acceleration::AbstractString,path_deceleration::AbstractStrin # y_axis = x[1]*x_axis + x[2] # plot(x_axis,y_axis) + + end diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index f709d966..3bf3695e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -257,10 +257,10 @@ function main() end log_step_diff[k+1,:] = step_diff - if lapStatus.currentLap > n_pf - if lapStatus.currentIt>1 - log_onestep[lapStatus.currentIt,:,lapStatus.currentLap] = abs(mpcSol.z[1:6] - zCurr[1:6]) - end + if size(mpcSol.z,2) > 5 + + log_onestep[lapStatus.currentIt,:,lapStatus.currentLap] = abs(mpcSol.z[2,1:6] - zCurr[i,1:6]) + end # ======================================= Lap trigger ======================================= diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 5afeaf64..8b61e336 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -491,7 +491,7 @@ type MpcModel_convhull @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) - #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity + #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # soft constraint on maximum velocity @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull #for n = 1:6 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 02ef97ed..d2c94c78 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,24 +47,24 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 6 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 3 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = true - mpcParams.N = 12 + mpcParams.N = 10 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f - mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivZ = 1.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs mpcParams.Q_term_cost = 4.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_lane = 10 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity mpcParams.Q_slack = [5.0,5.0,5.0,5.0,5.0,5.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories @@ -82,7 +82,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track trackCoeff.nPolyCurvature = 8 # 4th order polynomial for curvature approximation trackCoeff.coeffCurvature = zeros(trackCoeff.nPolyCurvature+1) # polynomial coefficients for curvature approximation (zeros for straight line) - trackCoeff.width = 0.6 # width of the track (0.5m) + trackCoeff.width = 0.6 # width of the track (60cm) modelParams.l_A = 0.125 modelParams.l_B = 0.125 From 595f5a42eca3a2c92eae5c63ce632629680e3a8b Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Fri, 26 Jan 2018 17:02:48 -0800 Subject: [PATCH 142/183] minor corrections on plots --- eval_sim/eval_data.jl | 2 +- workspace/src/barc/src/LMPC_node.jl | 6 +++--- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 2 +- workspace/src/barc/src/controller_low_level.py | 9 ++++++--- 4 files changed, 11 insertions(+), 8 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index b74ebd6b..befc00ae 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -490,7 +490,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) figure(20) clf() subplot(211) - plot(t,oldSS.oldSS[1:50,5,i],"-*") + plot(t,oldSS.oldSS[1:j,5,i],"-*") title("eY in lap $i, iteration $j") grid("on") plot(linspace(j,j+size(pred_sol)[1],size(pred_sol)[1]),pred_sol[:,5,j,i],"-+") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 3bf3695e..592d6ade 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -505,9 +505,9 @@ function main() log_input[lapStatus.currentIt,:,lapStatus.currentLap] = uCurr[i,:] log_mpcCost[lapStatus.currentIt,:,lapStatus.currentLap] = mpcSol.cost log_obs[lapStatus.currentIt,:,:,lapStatus.currentLap] = obs_curr[i,:,:] - # if lapStatus.currentLap > n_pf - # log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack - # end + if lapStatus.currentLap > n_pf + log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack + end #log_status[lapStatus.currentIt,lapStatus.currentLap] = mpcSol.solverStatus diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index d2c94c78..4b0705a0 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -52,7 +52,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) - selectedStates.version = true + selectedStates.version = false mpcParams.N = 10 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index b1a78aa3..99350954 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -51,14 +51,17 @@ def pwm_converter_callback(self, msg): elif FxR > 0: #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + + self.motor_pwm = max(94,90.74 + 6.17*FxR) #self.motor_pwm = 90.74 + 6.17*FxR - self.motor_pwm = max(94,90.12 + 5.24*FxR) + + #self.motor_pwm = max(94,90.12 + 5.24*FxR) #self.motor_pwm = 90.12 + 5.24*FxR # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down #self.motor_pwm = 93.5 + 46.73*FxR - #self.motor_pwm = 98.65 + 67.11*FxR - self.motor = 69.95 + 68.49*FxR + self.motor_pwm = 98.65 + 67.11*FxR + #self.motor = 69.95 + 68.49*FxR self.update_arduino() def neutralize(self): self.motor_pwm = 40 # slow down first From f08a99c36723e4264d9ad0feb1dedc577c69c6f5 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 29 Jan 2018 15:14:33 -0800 Subject: [PATCH 143/183] new tuning --- eval_sim/eval_data.jl | 15 +++++++++++++++ .../src/barc/launch/run_lmpc_remote.launch | 3 ++- workspace/src/barc/src/LMPC_node.jl | 4 ++-- workspace/src/barc/src/Localization_helpers.py | 16 ++++++++++++++++ .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 18 +++++++++--------- workspace/src/barc/src/controller_low_level.py | 8 ++++---- .../state_estimation_SensorKinematicModel.py | 8 ++++---- 8 files changed, 53 insertions(+), 21 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index befc00ae..5c08c53c 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1487,6 +1487,7 @@ function create_track(w) # add_curve(theta,35,0) # SIMPLE GOGGLE TRACK + add_curve(theta,30,0) add_curve(theta,40,-pi/2) add_curve(theta,10,0) @@ -1499,6 +1500,20 @@ function create_track(w) add_curve(theta,40,-pi/2) add_curve(theta,35,0) + # SIMPLE GOOGLE TRACK FOR 3110 + + # add_curve(theta,25,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,17,-pi/10) + # add_curve(theta,25,pi/5) + # add_curve(theta,17,-pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,32,0) + # OVAL TRACK FOR TESTS IN VSD # add_curve(theta,80,0) diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index f8eee292..8430f3fd 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -31,7 +31,8 @@ - + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 592d6ade..6972247d 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -168,7 +168,7 @@ function main() # Specific initializations: lapStatus.currentLap = 1 lapStatus.currentIt = 1 - posInfo.s_target = 19.11#19.14#17.94#17.76#24.0 + posInfo.s_target = 19.11 #17.91 #19.14#17.94#17.76#24.0 k = 0 # overall counter for logging mpcSol.z = zeros(11,4) @@ -272,7 +272,7 @@ function main() cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target #save the terminal cost for j = 1:buffersize - cost2target[j] = 1*(lapStatus.currentIt-j+1) + cost2target[j] = mpcParams.Q_term_cost*(lapStatus.currentIt-j+1) end oldSS.cost2target[:,lapStatus.currentLap-1] = cost2target diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index dd767dc2..ac6655ed 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -164,6 +164,7 @@ def create_track(self): # theta = add_curve(theta,37,0) # GOGGLE TRACK WITH STRAIGHT LINES, LENGTH = 19.11m (using ds = 0.03m) + theta = add_curve(theta,60,0) theta = add_curve(theta,80,-pi/2) theta = add_curve(theta,20,0) @@ -177,6 +178,21 @@ def create_track(self): theta = add_curve(theta,75,0) + # SMALL GOOGLE FOR 3110 + + # theta = add_curve(theta,50,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,35,-pi/10) + # theta = add_curve(theta,50,pi/5) + # theta = add_curve(theta,35,-pi/10) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,20,0) + # theta = add_curve(theta,80,-pi/2) + # theta = add_curve(theta,65,0) + + ###################################################### # TEST TRACKS diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 8b61e336..fae2e054 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -272,7 +272,7 @@ type MpcModel_pF # Set bounds z_lb_4s = ones(mpcParams.N+1,1)*[-Inf -Inf -Inf -0.5] # lower bounds on states z_ub_4s = ones(mpcParams.N+1,1)*[ Inf Inf Inf 1.5] # upper bounds - u_lb_4s = ones(mpcParams.N,1) * [0.0 -0.3] # lower bounds on steering + u_lb_4s = ones(mpcParams.N,1) * [0 -0.3] # lower bounds on steering u_ub_4s = ones(mpcParams.N,1) * [1.2 0.3] # upper bounds for i=1:2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 4b0705a0..10f7951f 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,8 +47,8 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.Nl = 3 # Number of previous laps to include in the convex hull + selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) @@ -59,22 +59,22 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f - mpcParams.QderivZ = 1.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 4.0 # scaling of Q-function + mpcParams.Q_term_cost = 1.0 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 10 # weight on the soft constraint for the lane + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = [5.0,5.0,5.0,5.0,5.0,5.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = [20.0,5.0,5.0,20.0,100.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories mpcParams_pF.N = 16 - mpcParams_pF.Q = [0.0,50.0,10.1,10.0] + mpcParams_pF.Q = [0.0,50.0,10.0,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f - mpcParams_pF.QderivZ = 0.0*[0,0,0.1,0] # cost matrix for derivative cost of states + mpcParams_pF.QderivZ = 1.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states mpcParams_pF.QderivU = 1*[10,10] # cost matrix for derivative cost of inputs mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) @@ -91,7 +91,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track modelParams.I_z = 0.03 modelParams.c_f = 0.5 # friction coefficient: xDot = - c_f*xDot (aerodynamic+tire) - posInfo.s_target = 19.11 + posInfo.s_target = 17.91 oldTraj.oldTraj = NaN*ones(buffersize,7,30) oldTraj.oldInput = zeros(buffersize,2,30) diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 99350954..870e515a 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -50,17 +50,17 @@ def pwm_converter_callback(self, msg): self.motor_pwm = 90.0 elif FxR > 0: #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino - #self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino - self.motor_pwm = max(94,90.74 + 6.17*FxR) + # self.motor_pwm = max(94,90.74 + 6.17*FxR) #self.motor_pwm = 90.74 + 6.17*FxR #self.motor_pwm = max(94,90.12 + 5.24*FxR) #self.motor_pwm = 90.12 + 5.24*FxR # Note: Barc doesn't move for u_pwm < 93 else: # motor break / slow down - #self.motor_pwm = 93.5 + 46.73*FxR - self.motor_pwm = 98.65 + 67.11*FxR + self.motor_pwm = 93.5 + 46.73*FxR + # self.motor_pwm = 98.65 + 67.11*FxR #self.motor = 69.95 + 68.49*FxR self.update_arduino() def neutralize(self): diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 701d7380..ff2d89fd 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -51,7 +51,7 @@ class StateEst(object): vel_meas = 0.0 vel_updated = False vel_prev = 0.0 - vel_count = 0.0 + vel_count = 0.0 # this counts how often the same vel measurement has been received # GPS x_meas = 0.0 @@ -144,6 +144,8 @@ def imu_callback(self, data): quaternion = (ori.x, ori.y, ori.z, ori.w) (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) # yaw_meas is element of [-pi,pi] + + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping # from this point on 'yaw' will be definitely unwrapped (smooth)! @@ -152,6 +154,7 @@ def imu_callback(self, data): self.yaw_meas = 0 # and current yaw to zero else: self.yaw_meas = yaw - self.yaw0 + #print "yaw measured: %f" % self.yaw_meas * 180 / math.pi # extract angular velocity and linear acceleration data #w_x = data.angular_velocity.x @@ -171,7 +174,6 @@ def imu_callback(self, data): self.imu_updated = True def encoder_vel_callback(self, data): - if data.vel_est != self.vel_prev: self.vel_meas = data.vel_est self.vel_updated = True @@ -182,8 +184,6 @@ def encoder_vel_callback(self, data): if self.vel_count > 10: # if 10 times in a row the same measurement self.vel_meas = 0 # set velocity measurement to zero self.vel_updated = True - - # self.vel_meas = data.vel_est # self.vel_updated = True From e4783d9be2cd708fc2b6e59066f09f1cc984b08c Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 30 Jan 2018 11:49:11 -0800 Subject: [PATCH 144/183] new tuning --- eval_sim/eval_data.jl | 10 +++++- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 32 +++++++++++-------- .../src/barc/src/barc_lib/LMPC/functions.jl | 6 ++-- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 10 +++--- .../state_estimation_SensorKinematicModel.py | 1 - 5 files changed, 35 insertions(+), 24 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 5c08c53c..0c55ed6c 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -209,7 +209,15 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) - # figure() + figure() + + plot(t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + title("Inputs") + grid("on") + legend(["a","d_f"]) + + + #figure() # subplot(221) # plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index fae2e054..d9fe6888 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -231,7 +231,7 @@ type MpcModel_pF uPrev::Array{JuMP.NonlinearParameter,2} - function MpcModel_pF(mpcParams::MpcParams,modelParams::ModelParams,trackCoeff::TrackCoeff) + function MpcModel_pF(mpcParams_pF::MpcParams,modelParams::ModelParams,trackCoeff::TrackCoeff) println("Starting creation of pf model") m = new() dt = modelParams.dt @@ -243,15 +243,17 @@ type MpcModel_pF z_lb = modelParams.z_lb z_ub = modelParams.z_ub - N = mpcParams.N - Q = mpcParams.Q - R = mpcParams.R - QderivZ = mpcParams.QderivZ::Array{Float64,1} - QderivU = mpcParams.QderivU::Array{Float64,1} - delay_df = mpcParams.delay_df::Int64 - delay_a = mpcParams.delay_a::Int64 + N = mpcParams_pF.N + Q = mpcParams_pF.Q + R = mpcParams_pF.R + QderivZ = mpcParams_pF.QderivZ::Array{Float64,1} + QderivU = mpcParams_pF.QderivU::Array{Float64,1} + delay_df = mpcParams_pF.delay_df::Int64 + delay_a = mpcParams_pF.delay_a::Int64 - v_ref = mpcParams.vPathFollowing + println("prediction h= ",N) + + v_ref = mpcParams_pF.vPathFollowing acc_f = 1.0 @@ -270,10 +272,10 @@ type MpcModel_pF @variable( mdl, u_Ol[1:N,1:2], start = 0) # Set bounds - z_lb_4s = ones(mpcParams.N+1,1)*[-Inf -Inf -Inf -0.5] # lower bounds on states - z_ub_4s = ones(mpcParams.N+1,1)*[ Inf Inf Inf 1.5] # upper bounds - u_lb_4s = ones(mpcParams.N,1) * [0 -0.3] # lower bounds on steering - u_ub_4s = ones(mpcParams.N,1) * [1.2 0.3] # upper bounds + z_lb_4s = ones(mpcParams_pF.N+1,1)*[-Inf -Inf -Inf -0.5] # lower bounds on states + z_ub_4s = ones(mpcParams_pF.N+1,1)*[ Inf Inf Inf 1.5] # upper bounds + u_lb_4s = ones(mpcParams_pF.N,1) * [0 -0.3] # lower bounds on steering + u_ub_4s = ones(mpcParams_pF.N,1) * [1.2 0.3] # upper bounds for i=1:2 for j=1:N @@ -437,7 +439,9 @@ type MpcModel_convhull delay_a = mpcParams.delay_a Q_slack = mpcParams.Q_slack - + println("prediction h= ",N) + + Np = selectedStates.Np::Int64 # how many states to select Nl = selectedStates.Nl::Int64 # how many previous laps to select diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 10f7951f..2ad7499c 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -74,8 +74,8 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.N = 16 mpcParams_pF.Q = [0.0,50.0,10.0,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f - mpcParams_pF.QderivZ = 1.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states - mpcParams_pF.QderivU = 1*[10,10] # cost matrix for derivative cost of inputs + mpcParams_pF.QderivZ = 0.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states + mpcParams_pF.QderivU = 1*[10,5*10] # cost matrix for derivative cost of inputs mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay @@ -91,7 +91,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track modelParams.I_z = 0.03 modelParams.c_f = 0.5 # friction coefficient: xDot = - c_f*xDot (aerodynamic+tire) - posInfo.s_target = 17.91 + posInfo.s_target = 19.11 oldTraj.oldTraj = NaN*ones(buffersize,7,30) oldTraj.oldInput = zeros(buffersize,2,30) diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index b884588f..ca6e92e4 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -72,17 +72,17 @@ function solveMpcProblem(mdl::MpcModel,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcPara nothing end -function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams::MpcParams,trackCoeff::TrackCoeff,posInfo::PosInfo, +function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,posInfo::PosInfo, modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},lapStatus::LapStatus) # Load Parameters coeffCurvature = trackCoeff.coeffCurvature::Array{Float64,1} - v_ref = mpcParams.vPathFollowing + v_ref = mpcParams_pF.vPathFollowing - z_ref1 = cat(2,zeros(mpcParams.N+1,3),v_ref*ones(mpcParams.N+1,1)) - z_ref2 = cat(2,zeros(mpcParams.N+1,1),0.2*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) - z_ref3 = cat(2,zeros(mpcParams.N+1,1),-0.1*ones(mpcParams.N+1,1),zeros(mpcParams.N+1,1),v_ref*ones(mpcParams.N+1,1)) + z_ref1 = cat(2,zeros(mpcParams_pF.N+1,3),v_ref*ones(mpcParams_pF.N+1,1)) + z_ref2 = cat(2,zeros(mpcParams_pF.N+1,1),0.2*ones(mpcParams_pF.N+1,1),zeros(mpcParams_pF.N+1,1),v_ref*ones(mpcParams_pF.N+1,1)) + z_ref3 = cat(2,zeros(mpcParams_pF.N+1,1),-0.1*ones(mpcParams_pF.N+1,1),zeros(mpcParams_pF.N+1,1),v_ref*ones(mpcParams_pF.N+1,1)) sol_status::Symbol sol_u::Array{Float64,2} diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index ff2d89fd..d1db8882 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -287,7 +287,6 @@ def state_estimation(): # Update track position l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) - #l.set_pos(se.x_meas, se.y_meas, psi_est_2, v_x_est, v_y_est, psi_dot_est) # Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz) if est_counter%4 == 0: From 120a3a90b5cc5f54cb7df14ab244cf1521bdec6a Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 30 Jan 2018 15:33:35 -0800 Subject: [PATCH 145/183] restored original track --- eval_sim/eval_data.jl | 6 +++--- workspace/src/barc/src/Localization_helpers.py | 6 +++--- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 0c55ed6c..9214031b 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1500,9 +1500,9 @@ function create_track(w) add_curve(theta,40,-pi/2) add_curve(theta,10,0) add_curve(theta,40,-pi/2) - add_curve(theta,20,-pi/10) - add_curve(theta,30,pi/5) - add_curve(theta,20,-pi/10) + add_curve(theta,20,pi/10) + add_curve(theta,30,-pi/5) + add_curve(theta,20,pi/10) add_curve(theta,40,-pi/2) add_curve(theta,10,0) add_curve(theta,40,-pi/2) diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index ac6655ed..bfc5ea13 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -169,9 +169,9 @@ def create_track(self): theta = add_curve(theta,80,-pi/2) theta = add_curve(theta,20,0) theta = add_curve(theta,80,-pi/2) - theta = add_curve(theta,40,-pi/10) - theta = add_curve(theta,60,pi/5) - theta = add_curve(theta,40,-pi/10) + theta = add_curve(theta,40,pi/10) + theta = add_curve(theta,60,-pi/5) + theta = add_curve(theta,40,pi/10) theta = add_curve(theta,80,-pi/2) theta = add_curve(theta,20,0) theta = add_curve(theta,80,-pi/2) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 2ad7499c..ba4a97d3 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -75,7 +75,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.Q = [0.0,50.0,10.0,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states - mpcParams_pF.QderivU = 1*[10,5*10] # cost matrix for derivative cost of inputs + mpcParams_pF.QderivU = 1*[10,10] # cost matrix for derivative cost of inputs mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay From b7e2a640970e08bcf44a90560074715a4b4f266b Mon Sep 17 00:00:00 2001 From: Ugo Date: Wed, 31 Jan 2018 19:18:26 -0800 Subject: [PATCH 146/183] New changes for new GPS pkg --- env_loader_pc.sh | 4 +- eval_sim/eval_data.jl | 3 + .../src/barc/launch/run_lmpc_remote.launch | 7 +- .../src/barc/src/barc_lib/log_functions.jl | 2 +- .../state_estimation_SensorKinematicModel.py | 4 +- workspace/src/barc/src/view_car_trajectory.py | 161 ++++++ .../src/ros_marvelmind_package/CHANGELOG.rst | 3 + .../src/ros_marvelmind_package/CMakeLists.txt | 2 + .../include/marvelmind_nav/marvelmind_hedge.h | 54 +- .../msg/beacon_pos_a.msg | 4 + .../ros_marvelmind_package/msg/hedge_pos.msg | 1 - .../msg/hedge_pos_a.msg | 6 + .../src/ros_marvelmind_package/package.xml | 2 +- .../src/hedge_rcv_bin.cpp | 102 +++- .../src/marvelmind_hedge.c | 523 +++++++++++++++--- .../src/subscriber_test.cpp | 71 ++- 16 files changed, 837 insertions(+), 112 deletions(-) create mode 100755 workspace/src/barc/src/view_car_trajectory.py create mode 100644 workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg create mode 100644 workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg diff --git a/env_loader_pc.sh b/env_loader_pc.sh index a9d68f9d..3b1281fb 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -2,6 +2,6 @@ source /opt/ros/kinetic/setup.bash source /home/ugo/GitHub/barc/workspace/devel/setup.bash export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=192.168.100.72 -export ROS_MASTER_URI=http://192.168.100.72:11311 +export ROS_IP=10.0.0.72 +export ROS_MASTER_URI=http://10.0.0.72:11311 exec "$@" \ No newline at end of file diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index d372c36c..56205dcf 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -405,14 +405,17 @@ function eval_LMPC(code::AbstractString) subplot(311) title("c_Vx") plot(t-t0,c_Vx) + legend(["1","2","3"]) grid("on") subplot(312) title("c_Vy") plot(t-t0,c_Vy) + legend(["1","2","3","4"]) grid("on") subplot(313) title("c_Psi") plot(t-t0,c_Psi) + legend(["1","2","3"]) grid("on") # *********** CURVATURE ********************* diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 7e53c1ae..aceb3e6f 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -7,8 +7,8 @@ - - + + @@ -36,6 +36,9 @@ + + + diff --git a/workspace/src/barc/src/barc_lib/log_functions.jl b/workspace/src/barc/src/barc_lib/log_functions.jl index 78195fb8..a56bf241 100644 --- a/workspace/src/barc/src/barc_lib/log_functions.jl +++ b/workspace/src/barc/src/barc_lib/log_functions.jl @@ -50,7 +50,7 @@ end function GPS_callback(msg::hedge_pos,gps_meas::Measurements) gps_meas.t[gps_meas.i] = to_sec(get_rostime()) - gps_meas.t_msg[gps_meas.i] = to_sec(msg.header.stamp) + gps_meas.t_msg[gps_meas.i] = msg.timestamp_ms gps_meas.z[gps_meas.i,:] = [msg.x_m;msg.y_m] gps_meas.i += 1 nothing diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index 57957331..f9b1430e 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -86,7 +86,9 @@ def gps_callback(self, data): """This function is called when a new GPS signal is received.""" # units: [rad] and [rad/s] t_now = rospy.get_rostime().to_sec()-self.t0 - t_msg = data.header.stamp.to_sec()-self.t0 + #t_msg = data.header.stamp.to_sec()-self.t0 + t_msg = t_now + # if abs(t_now - t_msg) > 0.1: # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) self.x_meas = data.x_m diff --git a/workspace/src/barc/src/view_car_trajectory.py b/workspace/src/barc/src/view_car_trajectory.py new file mode 100755 index 00000000..a747e363 --- /dev/null +++ b/workspace/src/barc/src/view_car_trajectory.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Author: J. Noonan +# Email: jpnoonan@berkeley.edu +# +# This code provides a way to see the car's trajectory, orientation, and velocity profile in +# real time with referenced to the track defined a priori. +# +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append, ones, polyval, delete, size, empty, linspace +from numpy import ones, polyval, delete, size +from observers import ekf +from system_models import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math +import matplotlib.pyplot as plt +import numpy as np +import pylab + +global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev +global pos_info_x_vals, pos_info_y_vals +global v_vals, t_vals, psi_vals + +gps_x_vals = [] +gps_y_vals = [] +gps_x_prev = 0.0 +gps_y_prev = 0.0 + +pos_info_x_vals = [] +pos_info_y_vals = [] + +v_vals = [] +t_vals = [] +psi_curr = 0.0 + +def gps_callback(data): + global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev + + dist = (gps_x_prev - data.x_m)**2 + (gps_y_prev - data.y_m)**2 + if dist < 1: + gps_x_vals.append(data.x_m) + gps_y_vals.append(data.y_m) + gps_x_prev = data.x_m + gps_y_prev = data.y_m + else: + gps_x_vals.append(gps_x_prev) + gps_y_vals.append(gps_y_prev) + + +def pos_info_callback(data): + global pos_info_x_vals, pos_info_y_vals + global v_vals, t_vals, psi_curr + + pos_info_x_vals.append(data.x) + pos_info_y_vals.append(data.y) + + v_vals.append(data.v) + t_vals.append(rospy.get_rostime().to_sec()) + psi_curr = data.psi + +def show(): + plt.show() + +def view_trajectory(): + + global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev + global pos_info_x_vals, pos_info_y_vals + global v_vals, t_vals, psi_curr + + rospy.init_node("car_view_trajectory_node", anonymous=True) + rospy.on_shutdown(show) + + rospy.Subscriber("hedge_pos", hedge_pos, gps_callback) + rospy.Subscriber("pos_info", pos_info, pos_info_callback) + + l = Localization() + l.create_track() + #l.create_circle(rad=0.8, c=array([0.0, -0.5])) + + fig = pylab.figure() + pylab.ion() + + vmax_ref = 1.0 + + loop_rate = 50 + rate = rospy.Rate(loop_rate) + + car_dx = 0.306 + car_dy = 0.177 + + car_xs_origin = [car_dx, car_dx, -car_dx, -car_dx, car_dx] + car_ys_origin = [car_dy, -car_dy, -car_dy, car_dy, car_dy] + + car_frame = np.vstack((np.array(car_xs_origin), np.array(car_ys_origin))) + while not rospy.is_shutdown(): + ax1 = fig.add_subplot(2, 1, 1) + ax2 = fig.add_subplot(2, 1, 2) + ax1.plot(l.nodes[0,:],l.nodes[1,:],"r-o") + ax1.grid('on') + ax1.axis('equal') + + + if (gps_x_vals and gps_y_vals): + ax1.plot(gps_x_vals[:len(gps_x_vals)-1], gps_y_vals[:len(gps_y_vals)-1], 'b-', label="GPS data path") + ax1.plot(gps_x_vals[len(gps_x_vals)-1], gps_y_vals[len(gps_y_vals)-1], 'b*',label="Car current GPS Pos") + + if (pos_info_x_vals and pos_info_y_vals): + ax1.plot(pos_info_x_vals[:len(pos_info_x_vals)-1], pos_info_y_vals[:len(pos_info_y_vals)-1], 'g-', label="Car path") + + x = pos_info_x_vals[len(pos_info_x_vals)-1] + y = pos_info_y_vals[len(pos_info_y_vals)-1] + ax1.plot(x, y, 'gs', label="Car current pos") + + R = np.matrix([[np.cos(psi_curr), -np.sin(psi_curr)], [np.sin(psi_curr), np.cos(psi_curr)]]) + + rotated_car_frame = R * car_frame + + car_xs = np.array(rotated_car_frame[0,:])[0] + car_ys = np.array(rotated_car_frame[1,:])[0] + + front_car_segment_x = np.array([car_xs[0], car_xs[1]]) + x + front_car_segment_y = np.array([car_ys[0], car_ys[1]]) + y + + ax1.plot(car_xs[1:] + x, car_ys[1:] + y, 'k-') + ax1.plot(front_car_segment_x, front_car_segment_y, 'y-') + #plt.plot(np.array(car_xs_origin) + x, np.array(car_ys_origin) + y, 'k-') + + if (v_vals): + t_vals_zeroed = [t - t_vals[0] for t in t_vals] + ax2.plot(t_vals_zeroed, v_vals, 'm-') + ax2.set_ylim([min(0, min(v_vals)), max(vmax_ref, max(v_vals))]) + + + ax1.set_title("Green = Data from POS_INFO, Blue = Data from GPS") + + ax2.set_xlabel("Time (s)") + ax2.set_ylabel("Velocity (m/s)") + + pylab.pause(0.001) + pylab.gcf().clear() + + + +if __name__ == '__main__': + try: + view_trajectory() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/ros_marvelmind_package/CHANGELOG.rst b/workspace/src/ros_marvelmind_package/CHANGELOG.rst index 93323d0f..b89c12a0 100644 --- a/workspace/src/ros_marvelmind_package/CHANGELOG.rst +++ b/workspace/src/ros_marvelmind_package/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package marvelmind_nav ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.6 (2017-02-10) +------------------ + 1.0.5 (2016-09-15) ------------------ diff --git a/workspace/src/ros_marvelmind_package/CMakeLists.txt b/workspace/src/ros_marvelmind_package/CMakeLists.txt index 77b706d6..41cc267b 100644 --- a/workspace/src/ros_marvelmind_package/CMakeLists.txt +++ b/workspace/src/ros_marvelmind_package/CMakeLists.txt @@ -51,6 +51,8 @@ add_message_files( DIRECTORY msg FILES hedge_pos.msg + hedge_pos_a.msg + beacon_pos_a.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h b/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h index 7888296e..8eb798f5 100644 --- a/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h +++ b/workspace/src/ros_marvelmind_package/include/marvelmind_nav/marvelmind_hedge.h @@ -5,9 +5,32 @@ struct PositionValue { + uint8_t address; uint32_t timestamp; - int16_t x, y, z; + int32_t x, y, z;// coordinates in millimeters uint8_t flags; + + bool highResolution; + + bool ready; + bool processed; +}; + +struct StationaryBeaconPosition +{ + uint8_t address; + int32_t x, y, z;// coordinates in millimeters + + bool updatedForMsg; + bool highResolution; +}; +#define MAX_STATIONARY_BEACONS 30 +struct StationaryBeaconsPositions +{ + uint8_t numBeacons; + struct StationaryBeaconPosition beacons[MAX_STATIONARY_BEACONS]; + + bool updated; }; struct MarvelmindHedge @@ -28,6 +51,8 @@ struct MarvelmindHedge // buffer of measurements struct PositionValue * positionBuffer; + + struct StationaryBeaconsPositions positionsBeacons; // verbose flag which activate console output // default: False @@ -44,6 +69,7 @@ struct MarvelmindHedge // private variables uint8_t lastValuesCount_; + uint8_t lastValues_next; bool haveNewValues_; #ifdef WIN32 HANDLE thread_; @@ -54,28 +80,26 @@ struct MarvelmindHedge #endif }; -#define POSITION_DATAGRAM_HEADER {0xff, 0x47, 0x01, 0x00, 0x10} -#define POSITION_DATAGRAM_HEADER_SIZE 5 - -#pragma pack (push, 1) -struct PositionDatagram_ -{ - uint8_t header[POSITION_DATAGRAM_HEADER_SIZE]; - uint32_t timestamp; - int16_t x, y, z; - uint8_t flags; - uint8_t align[5]; - uint16_t crc; -}; -#pragma pack (pop) +#define POSITION_DATAGRAM_ID 0x0001 +#define BEACONS_POSITIONS_DATAGRAM_ID 0x0002 +#define POSITION_DATAGRAM_HIGHRES_ID 0x0011 +#define BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012 struct MarvelmindHedge * createMarvelmindHedge (); void destroyMarvelmindHedge (struct MarvelmindHedge * hedge); void startMarvelmindHedge (struct MarvelmindHedge * hedge); + void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, bool onlyNew); bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, struct PositionValue * position); + +void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew); +bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct StationaryBeaconsPositions * positions); +void clearStationaryBeaconUpdatedFlag(struct MarvelmindHedge * hedge, uint8_t address); + void stopMarvelmindHedge (struct MarvelmindHedge * hedge); #ifdef WIN32 diff --git a/workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg b/workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg new file mode 100644 index 00000000..2f71c076 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/msg/beacon_pos_a.msg @@ -0,0 +1,4 @@ +uint8 address +float64 x_m +float64 y_m +float64 z_m diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg index 1d141d04..84a917dc 100644 --- a/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos.msg @@ -1,4 +1,3 @@ -Header header int64 timestamp_ms float64 x_m float64 y_m diff --git a/workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg b/workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg new file mode 100644 index 00000000..1c826020 --- /dev/null +++ b/workspace/src/ros_marvelmind_package/msg/hedge_pos_a.msg @@ -0,0 +1,6 @@ +uint8 address +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +uint8 flags diff --git a/workspace/src/ros_marvelmind_package/package.xml b/workspace/src/ros_marvelmind_package/package.xml index ab001a4d..a9aafe22 100644 --- a/workspace/src/ros_marvelmind_package/package.xml +++ b/workspace/src/ros_marvelmind_package/package.xml @@ -1,7 +1,7 @@ marvelmind_nav - 1.0.5 + 1.0.6 Marvelmind local navigation system diff --git a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp index ceebfde7..ecdf1656 100644 --- a/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp +++ b/workspace/src/ros_marvelmind_package/src/hedge_rcv_bin.cpp @@ -4,6 +4,8 @@ #include "ros/ros.h" #include "std_msgs/String.h" #include "marvelmind_nav/hedge_pos.h" +#include "marvelmind_nav/hedge_pos_a.h" +#include "marvelmind_nav/beacon_pos_a.h" extern "C" { #include "marvelmind_nav/marvelmind_hedge.h" @@ -12,12 +14,16 @@ extern "C" #include #define ROS_NODE_NAME "hedge_rcv_bin" -#define POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_ADDRESSED_TOPIC_NAME "/hedge_pos_a" +#define BEACONS_POSITION_ADDRESSED_TOPIC_NAME "/beacons_pos_a" struct MarvelmindHedge * hedge= NULL; static uint32_t hedge_timestamp_prev= 0; -marvelmind_nav::hedge_pos hedge_pos_msg;// message for publishing to ROS topic +marvelmind_nav::hedge_pos hedge_pos_noaddress_msg;// hedge coordinates message (old version without address) for publishing to ROS topic +marvelmind_nav::hedge_pos_a hedge_pos_msg;// hedge coordinates message for publishing to ROS topic +marvelmind_nav::beacon_pos_a beacon_pos_msg;// stationary beacon coordinates message for publishing to ROS topic //////////////////////////////////////////////////////////////////////// @@ -47,30 +53,77 @@ static bool hedgeReceiveCheck(void) struct PositionValue position; getPositionFromMarvelmindHedge (hedge, &position); + hedge_pos_msg.address= position.address; + hedge_pos_msg.flags= position.flags; + hedge_pos_noaddress_msg.flags= position.flags; if (hedge_pos_msg.flags&(1<<1))// flag of timestamp format { hedge_pos_msg.timestamp_ms= position.timestamp;// msec + hedge_pos_noaddress_msg.timestamp_ms= position.timestamp; } else { hedge_pos_msg.timestamp_ms= position.timestamp*15.625;// alpha-cycles ==> msec + hedge_pos_noaddress_msg.timestamp_ms= position.timestamp*15.625; } - hedge_pos_msg.header.stamp= ros::Time::now(); // added by mb for custom hedge_pos msg - hedge_pos_msg.x_m= position.x/100.0; - hedge_pos_msg.y_m= position.y/100.0; - hedge_pos_msg.z_m= position.z/100.0; + hedge_pos_msg.x_m= position.x/1000.0; + hedge_pos_msg.y_m= position.y/1000.0; + hedge_pos_msg.z_m= position.z/1000.0; + + hedge_pos_noaddress_msg.x_m= position.x/1000.0; + hedge_pos_noaddress_msg.y_m= position.y/1000.0; + hedge_pos_noaddress_msg.z_m= position.z/1000.0; hedge->haveNewValues_=false; + + return true; } + return false; +} + +static bool beaconReceiveCheck(void) +{ + uint8_t i; + struct StationaryBeaconsPositions positions; + struct StationaryBeaconPosition *bp= NULL; + bool foundUpd= false; + uint8_t n; + + getStationaryBeaconsPositionsFromMarvelmindHedge (hedge, &positions); + n= positions.numBeacons; + if (n == 0) + return false; + + for(i=0;iupdatedForMsg) + { + clearStationaryBeaconUpdatedFlag(hedge, bp->address); + foundUpd= true; + break; + } + } + if (!foundUpd) + return false; + if (bp == NULL) + return false; + + beacon_pos_msg.address= bp->address; + beacon_pos_msg.x_m= bp->x/1000.0; + beacon_pos_msg.y_m= bp->y/1000.0; + beacon_pos_msg.z_m= bp->z/1000.0; + + return true; } /** * Node for Marvelmind hedgehog binary streaming data processing */ int main(int argc, char **argv) -{ +{uint8_t beaconReadIterations; // initialize ROS node ros::init(argc, argv, ROS_NODE_NAME); @@ -80,18 +133,32 @@ int main(int argc, char **argv) // ROS node reference ros::NodeHandle n; - // Register topic for puplishing messages - ros::Publisher hedge_pos_publisher = n.advertise(POSITION_TOPIC_NAME, 1000); + // Register topics for puplishing messages + ros::Publisher hedge_pos_publisher = n.advertise(HEDGE_POSITION_ADDRESSED_TOPIC_NAME, 1000); + ros::Publisher hedge_pos_noaddress_publisher = n.advertise(HEDGE_POSITION_TOPIC_NAME, 1000); + ros::Publisher beacons_pos_publisher = n.advertise(BEACONS_POSITION_ADDRESSED_TOPIC_NAME, 1000); // 50 Hz ros::Rate loop_rate(50); // default values for position message + hedge_pos_msg.address= 0; hedge_pos_msg.timestamp_ms = 0; hedge_pos_msg.x_m = 0.0; hedge_pos_msg.y_m = 0.0; hedge_pos_msg.z_m = 0.0; hedge_pos_msg.flags = (1<<0);// 'data not available' flag + + hedge_pos_noaddress_msg.timestamp_ms = 0; + hedge_pos_noaddress_msg.x_m = 0.0; + hedge_pos_noaddress_msg.y_m = 0.0; + hedge_pos_noaddress_msg.z_m = 0.0; + hedge_pos_noaddress_msg.flags = (1<<0);// 'data not available' flag + + beacon_pos_msg.address= 0; + beacon_pos_msg.x_m = 0.0; + beacon_pos_msg.y_m = 0.0; + beacon_pos_msg.z_m = 0.0; while (ros::ok()) @@ -103,16 +170,29 @@ int main(int argc, char **argv) if (hedgeReceiveCheck()) {// hedgehog data received - ROS_INFO("%d, %d, %.3f, X=%.3f Y= %.3f Z=%.3f flags=%d", + ROS_INFO("Address: %d, timestamp: %d, %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.address, (int) hedge_pos_msg.timestamp_ms, (int) (hedge_pos_msg.timestamp_ms - hedge_timestamp_prev), - (float) hedge_pos_msg.header.stamp.toSec(), (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, (int) hedge_pos_msg.flags); hedge_pos_publisher.publish(hedge_pos_msg); + hedge_pos_noaddress_publisher.publish(hedge_pos_noaddress_msg); hedge_timestamp_prev= hedge_pos_msg.timestamp_ms; } + + beaconReadIterations= 0; + while(beaconReceiveCheck()) + {// stationary beacons data received + ROS_INFO("Stationary beacon: Address: %d, X=%.3f Y= %.3f Z=%.3f", + (int) beacon_pos_msg.address, + (float) beacon_pos_msg.x_m, (float) beacon_pos_msg.y_m, (float) beacon_pos_msg.z_m); + beacons_pos_publisher.publish(beacon_pos_msg); + + if ((beaconReadIterations++)>4) + break; + } ros::spinOnce(); diff --git a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c index 9bd2b103..e3544d08 100644 --- a/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c +++ b/workspace/src/ros_marvelmind_package/src/marvelmind_hedge.c @@ -12,8 +12,6 @@ #endif // WIN32 #include "marvelmind_nav/marvelmind_hedge.h" -uint8_t positionDatagramHeader[]=POSITION_DATAGRAM_HEADER; - ////////////////////////////////////////////////////////////////////////////// // Calculate CRC (Modbus) for array of bytes // buf: input buffer @@ -93,7 +91,7 @@ HANDLE OpenSerialPort_ (const char * portFileName, uint32_t baudrate, if (verbose) puts ("Error: unable to set serial port parameters"); CloseHandle (ttyHandle); return INVALID_HANDLE_VALUE; - } + } return ttyHandle; } #else @@ -218,6 +216,203 @@ int OpenSerialPort_ (const char * portFileName, uint32_t baudrate, bool verbose) } #endif +////////////////////////////////////////////////////////////////////////// + +static uint8_t markPositionReady(struct MarvelmindHedge * hedge) +{uint8_t ind= hedge->lastValues_next; + uint8_t indCur= ind; + + hedge->positionBuffer[ind].ready= + true; + hedge->positionBuffer[ind].processed= + false; + ind++; + if (ind>=hedge->maxBufferedPositions) + ind=0; + if (hedge->lastValuesCount_maxBufferedPositions) + hedge->lastValuesCount_++; + hedge->haveNewValues_=true; + + hedge->lastValues_next= ind; + + return indCur; +} + +static struct PositionValue process_position_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t ind= hedge->lastValues_next; + + hedge->positionBuffer[ind].address= + buffer[16]; + hedge->positionBuffer[ind].timestamp= + buffer[5] | + (((uint32_t ) buffer[6])<<8) | + (((uint32_t ) buffer[7])<<16) | + (((uint32_t ) buffer[8])<<24); + + int16_t vx= buffer[9] | + (((uint16_t ) buffer[10])<<8); + hedge->positionBuffer[ind].x= vx*10;// millimeters + + int16_t vy= buffer[11] | + (((uint16_t ) buffer[12])<<8); + hedge->positionBuffer[ind].y= vy*10;// millimeters + + int16_t vz= buffer[13] | + (((uint16_t ) buffer[14])<<8); + hedge->positionBuffer[ind].z= vz*10;// millimeters + + hedge->positionBuffer[ind].flags= buffer[15]; + + hedge->positionBuffer[ind].highResolution= false; + + ind= markPositionReady(hedge); + + return hedge->positionBuffer[ind]; +} + +static struct PositionValue process_position_highres_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t ind= hedge->lastValues_next; + + hedge->positionBuffer[ind].address= + buffer[22]; + hedge->positionBuffer[ind].timestamp= + buffer[5] | + (((uint32_t ) buffer[6])<<8) | + (((uint32_t ) buffer[7])<<16) | + (((uint32_t ) buffer[8])<<24); + + int32_t vx= buffer[9] | + (((uint32_t ) buffer[10])<<8) | + (((uint32_t ) buffer[11])<<16) | + (((uint32_t ) buffer[12])<<24); + hedge->positionBuffer[ind].x= vx; + + int32_t vy= buffer[13] | + (((uint32_t ) buffer[14])<<8) | + (((uint32_t ) buffer[15])<<16) | + (((uint32_t ) buffer[16])<<24); + hedge->positionBuffer[ind].y= vy; + + int32_t vz= buffer[17] | + (((uint32_t ) buffer[18])<<8) | + (((uint32_t ) buffer[19])<<16) | + (((uint32_t ) buffer[20])<<24); + hedge->positionBuffer[ind].z= vz; + + hedge->positionBuffer[ind].flags= buffer[21]; + + hedge->positionBuffer[ind].highResolution= true; + + ind= markPositionReady(hedge); + + return hedge->positionBuffer[ind]; +} + +static struct StationaryBeaconPosition *getOrAllocBeacon(struct MarvelmindHedge * hedge,uint8_t address) +{ + uint8_t i; + uint8_t n_used= hedge->positionsBeacons.numBeacons; + + if (n_used != 0) + for(i=0;ipositionsBeacons.beacons[i].address == address) + { + return &hedge->positionsBeacons.beacons[i]; + } + } + + if (n_used >= (MAX_STATIONARY_BEACONS-1)) + return NULL; + + hedge->positionsBeacons.numBeacons= (n_used + 1); + return &hedge->positionsBeacons.beacons[n_used]; +} + +static void process_beacons_positions_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{ + uint8_t n= buffer[5];// number of beacons in packet + uint8_t i,ofs; + uint8_t address; + int16_t x,y,z; + struct StationaryBeaconPosition *b; + + if ((1+n*8)!=buffer[4]) + return;// incorrect size + + for(i=0;iaddress= address; + b->x= x*10;// millimeters + b->y= y*10;// millimeters + b->z= z*10;// millimeters + + b->highResolution= false; + b->updatedForMsg= true; + + hedge->positionsBeacons.updated= true; + } + } +} + +static void process_beacons_positions_highres_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{ + uint8_t n= buffer[5];// number of beacons in packet + uint8_t i,ofs; + uint8_t address; + int32_t x,y,z; + struct StationaryBeaconPosition *b; + + if ((1+n*14)!=buffer[4]) + return;// incorrect size + + for(i=0;iaddress= address; + b->x= x; + b->y= y; + b->z= z; + + b->highResolution= true; + b->updatedForMsg= true; + + hedge->positionsBeacons.updated= true; + } + } +} + enum @@ -229,6 +424,7 @@ enum ////////////////////////////////////////////////////////////////////////////// // Thread function started by MarvelmindHedge_start ////////////////////////////////////////////////////////////////////////////// + void #ifndef WIN32 * @@ -236,11 +432,11 @@ void Marvelmind_Thread_ (void* param) { struct MarvelmindHedge * hedge=(struct MarvelmindHedge*) param; - struct PositionDatagram_ positionDatagram; + struct PositionValue curPosition; + uint8_t input_buffer[256]; uint8_t recvState=RECV_HDR; // current state of receive data uint8_t nBytesInBlockReceived=0; // bytes received - - uint8_t lastValues_next=0; + uint16_t dataId; SERIAL_PORT_HANDLE ttyHandle=OpenSerialPort_(hedge->ttyFileName, hedge->baudRate, hedge->verbose); @@ -253,7 +449,7 @@ Marvelmind_Thread_ (void* param) uint8_t receivedChar; bool readSuccessed=true; #ifdef WIN32 - uint32_t nBytesRead; + DWORD nBytesRead; readSuccessed=ReadFile (ttyHandle, &receivedChar, 1, &nBytesRead, NULL); #else int32_t nBytesRead; @@ -262,59 +458,90 @@ Marvelmind_Thread_ (void* param) #endif if (nBytesRead && readSuccessed) { - //printf("%x %d %d ", receivedChar & 0xff, nBytesRead, readSuccessed); - - *((char*)&positionDatagram+nBytesInBlockReceived)=receivedChar; + bool goodByte= false; + input_buffer[nBytesInBlockReceived]= receivedChar; switch (recvState) { case RECV_HDR: - if (receivedChar==positionDatagramHeader[nBytesInBlockReceived]) + switch(nBytesInBlockReceived) + { + case 0: + goodByte= (receivedChar == 0xff); + break; + case 1: + goodByte= (receivedChar == 0x47); + break; + case 2: + goodByte= true; + break; + case 3: + dataId= (((uint16_t) receivedChar)<<8) + input_buffer[2]; + goodByte= (dataId == POSITION_DATAGRAM_ID) || + (dataId == BEACONS_POSITIONS_DATAGRAM_ID) || + (dataId == POSITION_DATAGRAM_HIGHRES_ID) || + (dataId == BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID); + break; + case 4: + switch(dataId ) + { + case POSITION_DATAGRAM_ID: + goodByte= (receivedChar == 0x10); + break; + case BEACONS_POSITIONS_DATAGRAM_ID: + case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID: + goodByte= true; + break; + case POSITION_DATAGRAM_HIGHRES_ID: + goodByte= (receivedChar == 0x16); + break; + } + if (goodByte) + recvState=RECV_DGRAM; + break; + } + if (goodByte) { // correct header byte nBytesInBlockReceived++; - if (nBytesInBlockReceived>=sizeof(positionDatagramHeader)) - { - recvState=RECV_DGRAM; - } } else { // ...or incorrect + recvState=RECV_HDR; nBytesInBlockReceived=0; } break; case RECV_DGRAM: nBytesInBlockReceived++; - if (nBytesInBlockReceived>=sizeof(struct PositionDatagram_)) + if (nBytesInBlockReceived>=7+input_buffer[4]) { // parse dgram uint16_t blockCrc= - CalcCrcModbus_((uint8_t*)&positionDatagram, - sizeof(struct PositionDatagram_)-2); - if (blockCrc==positionDatagram.crc) + CalcCrcModbus_(input_buffer,nBytesInBlockReceived); + if (blockCrc==0) { - // add to positionBuffer #ifdef WIN32 EnterCriticalSection(&hedge->lock_); #else pthread_mutex_lock (&hedge->lock_); #endif - hedge->positionBuffer[lastValues_next].timestamp= - positionDatagram.timestamp; - hedge->positionBuffer[lastValues_next].x= - positionDatagram.x; - hedge->positionBuffer[lastValues_next].y= - positionDatagram.y; - hedge->positionBuffer[lastValues_next].z= - positionDatagram.z; - hedge->positionBuffer[lastValues_next].flags= - positionDatagram.flags; - lastValues_next++; - if (lastValues_next>=hedge->maxBufferedPositions) - lastValues_next=0; - if (hedge->lastValuesCount_maxBufferedPositions) - hedge->lastValuesCount_++; - hedge->haveNewValues_=true; + switch(dataId ) + { + case POSITION_DATAGRAM_ID: + // add to positionBuffer + curPosition= process_position_datagram(hedge, input_buffer); + break; + case BEACONS_POSITIONS_DATAGRAM_ID: + process_beacons_positions_datagram(hedge, input_buffer); + break; + case POSITION_DATAGRAM_HIGHRES_ID: + // add to positionBuffer + curPosition= process_position_highres_datagram(hedge, input_buffer); + break; + case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID: + process_beacons_positions_highres_datagram(hedge, input_buffer); + break; + } #ifdef WIN32 LeaveCriticalSection(&hedge->lock_); #else @@ -323,15 +550,10 @@ Marvelmind_Thread_ (void* param) // callback if (hedge->receiveDataCallback) { - struct PositionValue position= + if (dataId == POSITION_DATAGRAM_ID) { - positionDatagram.timestamp, - positionDatagram.x, - positionDatagram.y, - positionDatagram.z, - positionDatagram.flags - }; - hedge->receiveDataCallback (position); + hedge->receiveDataCallback (curPosition); + } } } // and repeat @@ -362,6 +584,7 @@ struct MarvelmindHedge * createMarvelmindHedge () hedge->verbose=false; hedge->receiveDataCallback=NULL; hedge->lastValuesCount_=0; + hedge->lastValues_next= 0; hedge->haveNewValues_=false; hedge->terminationRequired= false; #ifdef WIN32 @@ -378,7 +601,8 @@ struct MarvelmindHedge * createMarvelmindHedge () // Initialize and start work thread ////////////////////////////////////////////////////////////////////////////// void startMarvelmindHedge (struct MarvelmindHedge * hedge) -{ +{uint8_t i; + hedge->positionBuffer= malloc(sizeof (struct PositionValue)*hedge->maxBufferedPositions); if (hedge->positionBuffer==NULL) @@ -387,6 +611,18 @@ void startMarvelmindHedge (struct MarvelmindHedge * hedge) hedge->terminationRequired=true; return; } + for(i=0;imaxBufferedPositions;i++) + { + hedge->positionBuffer[i].ready= false; + hedge->positionBuffer[i].processed= false; + } + for(i=0;ipositionsBeacons.beacons[i].updatedForMsg= false; + } + hedge->positionsBeacons.numBeacons= 0; + hedge->positionsBeacons.updated= false; + #ifdef WIN32 _beginthread (Marvelmind_Thread_, 0, hedge); #else @@ -400,14 +636,16 @@ void startMarvelmindHedge (struct MarvelmindHedge * hedge) // position: pointer to PositionValue for write coordinates // returncode: true if position is valid ////////////////////////////////////////////////////////////////////////////// -bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, - struct PositionValue * position) + +static bool getPositionFromMarvelmindHedgeByAddress (struct MarvelmindHedge * hedge, + struct PositionValue * position, uint8_t address) { uint8_t i; - int16_t avg_x=0, avg_y=0, avg_z=0; + int32_t avg_x=0, avg_y=0, avg_z=0; uint32_t max_timestamp=0; - uint8_t flags= 0; bool position_valid; + bool highRes= false; + uint8_t flags= 0; #ifdef WIN32 EnterCriticalSection(&hedge->lock_); #else @@ -416,29 +654,47 @@ bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, if (hedge->lastValuesCount_) { uint8_t real_values_count=hedge->maxBufferedPositions; + uint8_t nFound= 0; if (hedge->lastValuesCount_lastValuesCount_; - position_valid=true; for (i=0; ipositionBuffer[i].address != address) + continue; + if (!hedge->positionBuffer[i].ready) + continue; + if (hedge->positionBuffer[i].processed) + continue; + if (address == 0) + address= hedge->positionBuffer[i].address; + nFound++; avg_x+=hedge->positionBuffer[i].x; avg_y+=hedge->positionBuffer[i].y; avg_z+=hedge->positionBuffer[i].z; - if (hedge->positionBuffer[i].timestamp>max_timestamp) - max_timestamp=hedge->positionBuffer[i].timestamp; - + flags= hedge->positionBuffer[i].flags; if (flags&(1<<0)) { position_valid=false; } + + if (hedge->positionBuffer[i].highResolution) + highRes= true; + hedge->positionBuffer[i].processed= true; + if (hedge->positionBuffer[i].timestamp>max_timestamp) + max_timestamp=hedge->positionBuffer[i].timestamp; + } + if (nFound != 0) + { + avg_x/=nFound; + avg_y/=nFound; + avg_z/=nFound; + position_valid=true; + } else + { + position_valid=false; } - avg_x/=real_values_count; - avg_y/=real_values_count; - avg_z/=real_values_count; - - if (!position_valid) - flags|= (1<<0);// coordiantes not available } else position_valid=false; #ifdef WIN32 @@ -446,28 +702,167 @@ bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, #else pthread_mutex_unlock (&hedge->lock_); #endif + if (!position_valid) + { + flags|= (1<<0);// coordiantes not available + } + position->address= address; position->x=avg_x; position->y=avg_y; position->z=avg_z; position->timestamp=max_timestamp; + position->ready= position_valid; + position->highResolution= highRes; position->flags= flags; return position_valid; } +bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct PositionValue * position) +{ + return getPositionFromMarvelmindHedgeByAddress(hedge, position, 0); +}; + ////////////////////////////////////////////////////////////////////////////// // Print average position coordinates // onlyNew: print only new positions ////////////////////////////////////////////////////////////////////////////// void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, bool onlyNew) -{ +{uint8_t i,j; + double xm,ym,zm; + if (hedge->haveNewValues_ || (!onlyNew)) { struct PositionValue position; - getPositionFromMarvelmindHedge (hedge, &position); - printf ("X: %d, Y: %d, Z: %d at time T: %u\n", position.x, - position.y, position.z, position.timestamp); - hedge->haveNewValues_=false; + uint8_t real_values_count=hedge->maxBufferedPositions; + uint8_t addresses[real_values_count]; + uint8_t addressesNum= 0; + + for(i=0;ipositionBuffer[i].address; + bool alreadyProcessed= false; + if (addressesNum != 0) + for(j=0;jhaveNewValues_=false; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Get positions of stationary beacons +// hedge: MarvelmindHedge structure +// positions: pointer to structure for write coordinates +////////////////////////////////////////////////////////////////////////////// + +bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct StationaryBeaconsPositions * positions) +{ +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + + *positions= hedge->positionsBeacons; + +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + + return true; +} + +void clearStationaryBeaconUpdatedFlag(struct MarvelmindHedge * hedge, uint8_t address) +{uint8_t i,n; + +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + + n= hedge->positionsBeacons.numBeacons; + + for(i=0;ipositionsBeacons.beacons[i].address == address) + { + hedge->positionsBeacons.beacons[i].updatedForMsg= false; + } + } + +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Print stationary beacons positions +// onlyNew: print only new positions +////////////////////////////////////////////////////////////////////////////// +void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew) +{struct StationaryBeaconsPositions positions; + double xm,ym,zm; + + getStationaryBeaconsPositionsFromMarvelmindHedge(hedge, &positions); + + if (positions.updated || (!onlyNew)) + {uint8_t i; + uint8_t n= hedge->positionsBeacons.numBeacons; + struct StationaryBeaconPosition *b; + + for(i=0;ix)/1000.0; + ym= ((double) b->y)/1000.0; + zm= ((double) b->z)/1000.0; + if (positions.beacons[i].highResolution) + { + printf ("Stationary beacon: address: %d, X: %.3f, Y: %.3f, Z: %.3f \n", + b->address,xm, ym, zm); + } else + { + printf ("Stationary beacon: address: %d, X: %.2f, Y: %.2f, Z: %.2f \n", + b->address,xm, ym, zm); + } + } + + hedge->positionsBeacons.updated= false; } } diff --git a/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp b/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp index b03d4ac8..c0edff17 100644 --- a/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp +++ b/workspace/src/ros_marvelmind_package/src/subscriber_test.cpp @@ -1,19 +1,26 @@ #include "ros/ros.h" #include "std_msgs/String.h" #include "marvelmind_nav/hedge_pos.h" +#include "marvelmind_nav/hedge_pos_a.h" +#include "marvelmind_nav/beacon_pos_a.h" #include #define ROS_NODE_NAME "subscriber_test" #define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_ADDRESSED_TOPIC_NAME "/hedge_pos_a" +#define BEACONS_POSITION_ADDRESSED_TOPIC_NAME "/beacons_pos_a" #define SCALE_HEDGE 3.0 ros::Publisher rviz_marker_pub; + uint32_t rviz_shape; +typedef enum {objHedge, objBeacon} DrawObjectType; -void showRvizObject(float x, float y, float z) -{ +void showRvizObject(uint8_t address, float x, float y, float z, DrawObjectType obj) +{uint8_t lifeTime, i; + if (rviz_marker_pub.getNumSubscribers() < 1) return; visualization_msgs::Marker marker; @@ -24,7 +31,7 @@ void showRvizObject(float x, float y, float z) // Set the namespace and id for this marker. This serves to create a unique ID // Any marker sent with the same namespace and id will overwrite the old one marker.ns = "basic_shapes"; - marker.id = 0; + marker.id = address; // Set the marker type marker.type = rviz_shape; @@ -46,29 +53,63 @@ void showRvizObject(float x, float y, float z) marker.scale.y = 0.05*SCALE_HEDGE; marker.scale.z = 0.02*SCALE_HEDGE; // Set the color -- be sure to set alpha to something non-zero! - marker.color.r = 0.0f; - marker.color.g = 1.0f; - marker.color.b = 0.0f; + if (obj == objHedge) + { + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + } + else + { + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + } marker.color.a = 1.0; - marker.lifetime = ros::Duration(5); + if (obj == objHedge) lifeTime= 5; + else lifeTime= 25; + marker.lifetime = ros::Duration(lifeTime); rviz_marker_pub.publish(marker); } -void hedgePosCallback(const marvelmind_nav::hedge_pos& hedge_pos_msg) +void hedgePosCallback(const marvelmind_nav::hedge_pos_a& hedge_pos_msg) +{ + ROS_INFO("Hedgehog data: Address= %d, timestamp= %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.address, + (int) hedge_pos_msg.timestamp_ms, + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (int) hedge_pos_msg.flags); + + if ((hedge_pos_msg.flags&(1<<0))==0) + { + showRvizObject(hedge_pos_msg.address,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); + } +} + +void hedgePos_noaddressCallback(const marvelmind_nav::hedge_pos& hedge_pos_msg) { - ROS_INFO("Hedgehog data: %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + ROS_INFO("Hedgehog data: Timestamp= %d, X=%.3f Y= %.3f Z=%.3f flags=%d", (int) hedge_pos_msg.timestamp_ms, (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, (int) hedge_pos_msg.flags); if ((hedge_pos_msg.flags&(1<<0))==0) { - showRvizObject(hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m); + showRvizObject(0,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); } } +void beaconsPosCallback(const marvelmind_nav::beacon_pos_a& beacon_pos_msg) +{ + ROS_INFO("Stationary beacon data: Address= %d, X=%.3f Y= %.3f Z=%.3f", + (int) beacon_pos_msg.address, + (float) beacon_pos_msg.x_m, (float) beacon_pos_msg.y_m, (float) beacon_pos_msg.z_m); + + showRvizObject(beacon_pos_msg.address, beacon_pos_msg.x_m, beacon_pos_msg.y_m, beacon_pos_msg.z_m, objBeacon); +} + /** * Test subscriber node for getting data from Marvelmind publishers nodes */ @@ -77,15 +118,17 @@ int main(int argc, char **argv) // initialize ROS node ros::init(argc, argv, ROS_NODE_NAME); - + // ROS node reference - ros::NodeHandle n; + ros::NodeHandle rosNode; // Declare need to subscribe data from topic - ros::Subscriber sub = n.subscribe(HEDGE_POSITION_TOPIC_NAME, 1000, hedgePosCallback); + ros::Subscriber subHedge = rosNode.subscribe(HEDGE_POSITION_ADDRESSED_TOPIC_NAME, 1000, hedgePosCallback); + //ros::Subscriber subHedge_noaddress = rosNode.subscribe(HEDGE_POSITION_TOPIC_NAME, 1000, hedgePos_noaddressCallback); + ros::Subscriber subBeacons = rosNode.subscribe(BEACONS_POSITION_ADDRESSED_TOPIC_NAME, 1000, beaconsPosCallback); // Declare publisher for rviz visualization - rviz_marker_pub = n.advertise("visualization_marker", 1); + rviz_marker_pub = rosNode.advertise("visualization_marker", 1); // Set our initial shape type to be a cube rviz_shape = visualization_msgs::Marker::CUBE; From a8b0ba059a51d47e54cda76e30594531e896940d Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 1 Feb 2018 16:41:35 -0800 Subject: [PATCH 147/183] tuning --- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 4 ++-- workspace/src/barc/src/open_loop.jl | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index ba4a97d3..b1c63fa4 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -72,10 +72,10 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams_pF.N = 16 - mpcParams_pF.Q = [0.0,50.0,10.0,10.0] + mpcParams_pF.Q = [0.0,20.0,1.0,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states - mpcParams_pF.QderivU = 1*[10,10] # cost matrix for derivative cost of inputs + mpcParams_pF.QderivU = 1*[10,1] # cost matrix for derivative cost of inputs mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay diff --git a/workspace/src/barc/src/open_loop.jl b/workspace/src/barc/src/open_loop.jl index 533faf69..299b7352 100755 --- a/workspace/src/barc/src/open_loop.jl +++ b/workspace/src/barc/src/open_loop.jl @@ -241,7 +241,7 @@ function main() t0 = to_sec(t0_ros) t = 0.0 - t_next = 2 + t_next = 2 # SET THIS TO 4 IF YOU DO ACCELERATION TESTS gps_meas.i = 1 imu_meas.i = 1 @@ -250,9 +250,9 @@ function main() vel_est_log.i = 1 pos_info_log.i = 1 - cmd_m = 100 + cmd_m = 100 # PWM COMMAND FOR MOTOR - cmd_s = 93 + cmd_s = 93 # PWM COMMAND FOR STEERING # t_cmd = zeros(10) # t_no_cmd = zeros(10) From 1d27f32688da0706c5615281eb564cb727b0dc92 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 5 Feb 2018 08:41:43 -0800 Subject: [PATCH 148/183] tuning for simulator --- eval_sim/eval_data.jl | 24 +++---- workspace/src/barc/src/LMPC_node.jl | 15 ++-- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 5 +- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 6 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 72 +++++++++++++------ workspace/src/barc/src/barc_lib/classes.jl | 5 +- 6 files changed, 82 insertions(+), 45 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 9214031b..c9d27868 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -376,22 +376,22 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) oldvx = selStates[1:Np,1,j,i] oldvx2 = selStates[Np+1:2*Np,1,j,i] - oldvx3 = selStates[2*Np+1:3*Np,1,j,i] + #oldvx3 = selStates[2*Np+1:3*Np,1,j,i] oldvy = selStates[1:Np,2,j,i] oldvy2 = selStates[Np+1:2*Np,2,j,i] - oldvy3 = selStates[2*Np+1:3*Np,2,j,i] + #oldvy3 = selStates[2*Np+1:3*Np,2,j,i] oldpsiDot = selStates[1:Np,3,j,i] oldpsiDot2 = selStates[Np+1:2*Np,3,j,i] - oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] + #oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] oldePsi = selStates[1:Np,4,j,i] oldePsi2 = selStates[Np+1:2*Np,4,j,i] - oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] + #oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] oldeY = selStates[1:Np,5,j,i] oldeY2 = selStates[Np+1:2*Np,5,j,i] - oldeY3 = selStates[2*Np+1:3*Np,5,j,i] + #oldeY3 = selStates[2*Np+1:3*Np,5,j,i] olds = selStates[1:Np,6,j,i] olds2 = selStates[Np+1:2*Np,6,j,i] - olds3 = selStates[2*Np+1:3*Np,6,j,i] + #olds3 = selStates[2*Np+1:3*Np,6,j,i] t = linspace(1,j,j) @@ -422,7 +422,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,vx_pred,"or") plot(olds,oldvx,"b") plot(olds2,oldvx2,"b") - plot(olds3,oldvx3,"b") + #plot(olds3,oldvx3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) title("State vx in lap $i, iteration $j") grid("on") @@ -431,7 +431,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,vy_pred,"or") plot(olds,oldvy,"b") plot(olds2,oldvy2,"b") - plot(olds3,oldvy3,"b") + #plot(olds3,oldvy3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) title("State vy in lap $i, iteration $j ") grid("on") @@ -440,7 +440,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,psiDot_pred,"or") plot(olds,oldpsiDot,"b") plot(olds2,oldpsiDot2,"b") - plot(olds3,oldpsiDot3,"b") + #plot(olds3,oldpsiDot3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) title("State psiDot in lap $i , iteration $j") grid("on") @@ -449,7 +449,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,ePsi_pred,"or") plot(olds,oldePsi,"b") plot(olds2,oldePsi2,"b") - plot(olds3,oldePsi3,"b") + #plot(olds3,oldePsi3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) title("State ePsi in lap $i, iteration $j ") grid("on") @@ -461,7 +461,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(s_pred,eY_pred,"or") plot(olds,oldeY,"b") plot(olds2,oldeY2,"b") - plot(olds3,oldeY3,"b") + #plot(olds3,oldeY3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) title("State eY in lap $i, iteration $j ") grid("on") @@ -512,7 +512,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) - sleep(5) + sleep(1) end end end diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 6972247d..697ccc94 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -92,9 +92,16 @@ function main() InitializeParameters(mpcParams,mpcParams_pF,trackCoeff,modelParams,posInfo,oldTraj,mpcCoeff,lapStatus,buffersize,obstacle,selectedStates,oldSS) - mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) mdl_pF = MpcModel_pF(mpcParams_pF,modelParams,trackCoeff) - mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + + if selectedStates.version == true + mdl = MpcModel(mpcParams,mpcCoeff,modelParams,trackCoeff) + elseif selectedStates.version == false + mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) + end + + + #mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) mdl_obstacle = MpcModel_obstacle(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates,obstacle) @@ -109,8 +116,8 @@ function main() coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables - log_coeff_Cost = NaN*ones(mpcCoeff.order+1,2,10000) # DONT NEED THIS - log_coeff_Const = NaN*ones(mpcCoeff.order+1,2,5,10000) # DONT NEED THIS + log_coeff_Cost = NaN*ones(mpcCoeff.order+1,2,10000) + log_coeff_Const = NaN*ones(mpcCoeff.order+1,2,5,10000) log_sol_z = NaN*ones(max_N+1,7,10000) log_sol_u = NaN*ones(max_N,2,10000) log_curv = zeros(10000,trackCoeff.nPolyCurvature+1) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index d9fe6888..55a85599 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -439,7 +439,7 @@ type MpcModel_convhull delay_a = mpcParams.delay_a Q_slack = mpcParams.Q_slack - println("prediction h= ",N) + println("prediction horizon= ",N) Np = selectedStates.Np::Int64 # how many states to select @@ -564,7 +564,8 @@ type MpcModel_convhull # Lane cost (soft) # --------------------------------- - @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) + @NLexpression(mdl, laneCost, Q_lane*sum{30.0*eps_lane[i]+200.0*eps_lane[i]^2 ,i=2:N+1}) + # Terminal Cost # --------------------------------- diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index f4c49e44..64de5e4b 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -56,9 +56,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo selected_laps[i] = lapStatus.currentLap-i # use previous lap end - if lapStatus.currentLap >= 5 - selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2]) # and the best from all previous laps - end + # if lapStatus.currentLap >= 5 + # selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2]) # and the best from all previous laps + # end # Select the old data oldxDot = oldTraj.oldTraj[:,1,selected_laps]::Array{Float64,3} diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index b1c63fa4..6840dfad 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,28 +46,56 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - - selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.Nl = 2 # Number of previous laps to include in the convex hull - Nl = selectedStates.Nl - selectedStates.selStates = zeros(Nl*selectedStates.Np,6) - selectedStates.statesCost = zeros(Nl*selectedStates.Np) - selectedStates.version = false - - mpcParams.N = 10 - mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) - mpcParams.vPathFollowing = 1 # reference speed for first lap of path following - mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) - mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f - mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 1.0 # scaling of Q-function - mpcParams.delay_df = 3 # steering delay - mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 1 # weight on the soft constraint for the lane - mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = [20.0,5.0,5.0,20.0,100.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s - mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + selectedStates.simulator == true + + if selectedStates.simulator == false # if the BARC is in use + + selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + Nl = selectedStates.Nl + selectedStates.selStates = zeros(Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(Nl*selectedStates.Np) + selectedStates.version = false + + mpcParams.N = 10 + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 1 # reference speed for first lap of path following + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 1.0 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + mpcParams.Q_slack = [20.0,5.0,5.0,20.0,100.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + + elseif selectedStates.simulator == true # if the simulator is in use + + selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + Nl = selectedStates.Nl + selectedStates.selStates = zeros(Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(Nl*selectedStates.Np) + selectedStates.version = false + + mpcParams.N = 10 + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 1 # reference speed for first lap of path following + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + mpcParams.Q_slack = 0.5*[20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + end diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index 02c2cfde..badf195c 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -59,8 +59,9 @@ type SelectedStates # Values needed for the convex hull formulat statesCost::Array{Float64} # ... and their related costs Np::Int64 # number of states to select from each previous lap Nl::Int64 # number of previous laps to include in the convex hull - version::Bool - SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false) = new(selStates,statesCost,Np,Nl,version) + version::Bool # false if you want to use convex hull + simulator::Bool # true to use tuning made for simulator, false to use tuning made for BARC + SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false, simulator=true) = new(selStates,statesCost,Np,Nl,version,simulator) end type MpcParams # parameters for MPC solver From 56f2e5ef4b640e4ca798c21238314d734ed122ea Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Tue, 6 Feb 2018 05:28:10 -0800 Subject: [PATCH 149/183] new tuning for experiments --- env_loader_pc.sh | 8 +- eval_sim/eval_data.jl | 77 +++++++++++++++---- .../src/barc/launch/run_lmpc_remote.launch | 2 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 10 +-- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 9 +-- .../src/barc/src/barc_lib/LMPC/functions.jl | 22 +++--- workspace/src/barc/src/barc_lib/classes.jl | 3 +- .../state_estimation_SensorKinematicModel.py | 1 + 8 files changed, 92 insertions(+), 40 deletions(-) diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 4889e2d3..139f04b3 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -1,7 +1,7 @@ #!/bin/bash source /opt/ros/kinetic/setup.bash -source /home/ugo/GitHub/barc/workspace/devel/setup.bash -export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=10.0.0.72 -export ROS_MASTER_URI=http://10.0.0.72:11311 +source /home/mpcubuntu/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/ugo/julia/bin/julia +export ROS_IP=10.0.0.14 +export ROS_MASTER_URI=http://10.0.0.14:11311 exec "$@" diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index c9d27868..9bcf2c6b 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -131,9 +131,54 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) flag = zeros(2) - track = create_track(0.3) + track = create_track(0.4) println("prediction horizon N= ", size(pred_sol)[1]) + + ospe1 = [] + ospe2 = [] + ospe3 = [] + ospe4 = [] + ospe5 = [] + ospe6 = [] + + slack1 = [] + slack2 = [] + slack3 = [] + slack4 = [] + slack5 = [] + slack6 = [] + + for l = laps + ospe1 = vcat(ospe1,one_step_error[1:currentIt,1,l]) + ospe2 = vcat(ospe2,one_step_error[1:currentIt,2,l]) + ospe3 = vcat(ospe3,one_step_error[1:currentIt,3,l]) + ospe4 = vcat(ospe4,one_step_error[1:currentIt,4,l]) + ospe5 = vcat(ospe5,one_step_error[1:currentIt,5,l]) + ospe6 = vcat(ospe6,one_step_error[1:currentIt,6,l]) + + slack1 = vcat(slack1,costSlack[1:currentIt,1,l]) + slack2 = vcat(slack2,costSlack[1:currentIt,2,l]) + slack3 = vcat(slack3,costSlack[1:currentIt,3,l]) + slack4 = vcat(slack4,costSlack[1:currentIt,4,l]) + slack5 = vcat(slack5,costSlack[1:currentIt,5,l]) + slack6 = vcat(slack6,costSlack[1:currentIt,6,l]) + end + + t_ospe = linspace(1,length(ospe1),length(ospe1)) + + figure() + plot(t_ospe,ospe1,t_ospe,ospe2,t_ospe,ospe3,t_ospe,ospe4,t_ospe,ospe5,t_ospe,ospe6) + legend(["vx","vy","psiDot","ePsi","eY","s"]) + grid("on") + title("One step prediction errors") + + figure() + plot(t_ospe,slack1,t_ospe,slack2,t_ospe,slack3,t_ospe,slack4,t_ospe,slack5,t_ospe,slack6) + legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + title("Slack costs") + grid("on") + @@ -170,7 +215,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cpsi2 = cpsi[1:currentIt,2,i] cpsi3 = cpsi[1:currentIt,3,i] - figure(1) + figure() plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") @@ -212,7 +257,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) figure() plot(t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") - title("Inputs") + title("Inputs in lap $i") grid("on") legend(["a","d_f"]) @@ -264,12 +309,21 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) # legend(["ospe","a_x","d_f"]) # title("One step prediction error for s in lap $i") # grid("on") + + + # figure() + # plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i]) + # legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + # title("Slack costs") + # grid("on") - figure() - plot(t,one_step_error[1:currentIt,1,i],t,one_step_error[1:currentIt,2,i],t,one_step_error[1:currentIt,3,i],t,one_step_error[1:currentIt,4,i],t,one_step_error[1:currentIt,5,i],t,one_step_error[1:currentIt,6,i]) - legend(["vx","vy","psiDot","ePsi","eY","s"]) - grid("on") - title("One step prediction errors") + + + # figure() + # plot(t,one_step_error[1:currentIt,1,i],t,one_step_error[1:currentIt,2,i],t,one_step_error[1:currentIt,3,i],t,one_step_error[1:currentIt,4,i],t,one_step_error[1:currentIt,5,i],t,one_step_error[1:currentIt,6,i]) + # legend(["vx","vy","psiDot","ePsi","eY","s"]) + # grid("on") + # title("One step prediction errors") # println("one step prediction error= ",one_step_error[1:30,:,i]) @@ -344,14 +398,9 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) figure() plot(t,cost[1:currentIt,2,i],t,cost[1:currentIt,3,i],t,cost[1:currentIt,4,i],t,cost[1:currentIt,6,i]) legend(["terminal Cost","control Cost","derivative Cost","lane Cost"]) - title("Costs of the Mpc") + title("Costs of the Mpc in lap $i") grid("on") - figure() - plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i]) - legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) - title("Slack costs") - grid("on") diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 76b4c34d..7520d18d 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -9,7 +9,7 @@ - + diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 55a85599..152a104f 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -540,11 +540,11 @@ type MpcModel_convhull # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) # end - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.12) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.12) for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.12) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.12) end @@ -564,7 +564,7 @@ type MpcModel_convhull # Lane cost (soft) # --------------------------------- - @NLexpression(mdl, laneCost, Q_lane*sum{30.0*eps_lane[i]+200.0*eps_lane[i]^2 ,i=2:N+1}) + @NLexpression(mdl, laneCost, Q_lane*sum{40.0*eps_lane[i]+300.0*eps_lane[i]^2 ,i=2:N+1}) # Terminal Cost diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index 64de5e4b..07afdb79 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -56,9 +56,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo selected_laps[i] = lapStatus.currentLap-i # use previous lap end - # if lapStatus.currentLap >= 5 - # selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2]) # and the best from all previous laps - # end + if lapStatus.currentLap >= 5 + selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2]) # and the best from all previous laps + end # Select the old data oldxDot = oldTraj.oldTraj[:,1,selected_laps]::Array{Float64,3} @@ -100,8 +100,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! idx_s2= findmin(DistS2,1)[2] - off = 3 - idx_s2 = idx_s2 + off + idx_s2 = idx_s2 + selectedStates.shift # Propagate the obstacle for the prediction horizon diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 6840dfad..19f02478 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,30 +46,32 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.simulator == true + selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use if selectedStates.simulator == false # if the BARC is in use - selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 23 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.shift = 7 selectedStates.Nl = 2 # Number of previous laps to include in the convex hull Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 10 + + mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following - mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY) + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 1.0 # scaling of Q-function + mpcParams.QderivU = 1.0*[5.0,1.0] #a_x,delta_f # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 2 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = [20.0,5.0,5.0,20.0,100.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 1*[20.0,20.0,10.0,30.0,80.0,1*50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories elseif selectedStates.simulator == true # if the simulator is in use @@ -93,17 +95,17 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 0.5*[20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 1*[20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories end mpcParams_pF.N = 16 - mpcParams_pF.Q = [0.0,20.0,1.0,10.0] + mpcParams_pF.Q = [0.0,50.0,1.0,10.0] mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f mpcParams_pF.QderivZ = 0.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states - mpcParams_pF.QderivU = 1*[10,1] # cost matrix for derivative cost of inputs + mpcParams_pF.QderivU = 10*[1,1] # cost matrix for derivative cost of inputs mpcParams_pF.vPathFollowing = 1 # reference speed for first lap of path following mpcParams_pF.delay_df = 3 # steering delay (number of steps) mpcParams_pF.delay_a = 1 # acceleration delay diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index badf195c..f6d85246 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -61,7 +61,8 @@ type SelectedStates # Values needed for the convex hull formulat Nl::Int64 # number of previous laps to include in the convex hull version::Bool # false if you want to use convex hull simulator::Bool # true to use tuning made for simulator, false to use tuning made for BARC - SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false, simulator=true) = new(selStates,statesCost,Np,Nl,version,simulator) + shift::Int64 + SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false, simulator=true,shift=3) = new(selStates,statesCost,Np,Nl,version,simulator,shift) end type MpcParams # parameters for MPC solver diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index f6ba1715..f3cd89c0 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -224,6 +224,7 @@ def state_estimation(): Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + #R = diag([10.0,10.0,1.0,10.0,100.0,1000.0,1000.0, 10.0,10.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters From 49dbeb8763265bc82ce3e6a4117ec291d3b764c2 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Wed, 7 Feb 2018 14:59:33 -0800 Subject: [PATCH 150/183] added solver status to plots --- eval_sim/eval_data.jl | 70 ++++++++++--------- workspace/src/barc/src/LMPC_node.jl | 8 ++- .../src/barc/src/barc_lib/LMPC/functions.jl | 8 +-- 3 files changed, 45 insertions(+), 41 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 9bcf2c6b..db32d60c 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -96,7 +96,7 @@ function eval_sim(code::AbstractString) grid() end -function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) +function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obstacle::Bool) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" @@ -121,7 +121,8 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) costSlack = Data["mpcCostSlack"] obs_log = Data["obs_log"] sol_u = Data["sol_u"] - #status = Data["status"] + final_counter = Data["final_counter"] + status = Data["sol_status"] Nl = selectedStates.Nl Np = selectedStates.Np @@ -133,7 +134,9 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) track = create_track(0.4) - println("prediction horizon N= ", size(pred_sol)[1]) + pred_horizon = size(pred_sol)[1] - 1 + + println("prediction horizon N= ", pred_horizon) ospe1 = [] ospe2 = [] @@ -215,34 +218,28 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) cpsi2 = cpsi[1:currentIt,2,i] cpsi3 = cpsi[1:currentIt,3,i] - figure() - plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") - #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") - plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + if obstacle == false + figure() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - # for index=1:length(oldSS_xy[:,1,i]) - # z_pred = zeros(size(pred_sol)[1],4) - # #z_pred[1,:] = x_est[i,:] - # z_pred[1,:] = oldSS_xy[index,:,i] - # for j=2:size(pred_sol)[1] - # z_pred[j,:] = simModel(z_pred[j-1,:],pred_input[j-1,:,index,i],0.1,0.125,0.125) - # end - # plot(z_pred[:,1],z_pred[:,2],"-+") - # end + elseif obstacle == true - # ellfig = figure(1) - # ax = ellfig[:add_subplot](1,1,1) - # ax[:set_aspect]("equal") - # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"og") - # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") - # plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + ellfig = figure(3) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - # angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) - # angle_deg = (angle_ell*180)/pi + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi - # ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 0)#angle=angle_deg) - # ax[:add_artist](ell1) + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 0)#angle=angle_deg) + ax[:add_artist](ell1) + end grid("on") @@ -413,7 +410,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) for j = 2:2000 - + solution_status = status[final_counter[i-1]+j] vx_pred = pred_sol[:,1,j,i] vy_pred = pred_sol[:,2,j,i] @@ -473,7 +470,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(olds2,oldvx2,"b") #plot(olds3,oldvx3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) - title("State vx in lap $i, iteration $j") + title("State vx in lap $i, iteration $j, status = $solution_status") grid("on") subplot(222) @@ -482,7 +479,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(olds2,oldvy2,"b") #plot(olds3,oldvy3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) - title("State vy in lap $i, iteration $j ") + title("State vy in lap $i, iteration $j, status = $solution_status ") grid("on") subplot(223) @@ -491,7 +488,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(olds2,oldpsiDot2,"b") #plot(olds3,oldpsiDot3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) - title("State psiDot in lap $i , iteration $j") + title("State psiDot in lap $i , iteration $j, status = $solution_status") grid("on") subplot(224) @@ -500,7 +497,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(olds2,oldePsi2,"b") #plot(olds3,oldePsi3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) - title("State ePsi in lap $i, iteration $j ") + title("State ePsi in lap $i, iteration $j, status = $solution_status ") grid("on") @@ -512,7 +509,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(olds2,oldeY2,"b") #plot(olds3,oldeY3,"b") #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) - title("State eY in lap $i, iteration $j ") + title("State eY in lap $i, iteration $j, status $solution_status ") grid("on") figure(17) @@ -523,8 +520,11 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) plot(t,velocity,"-*") legend(["velocity"]) + subplot(122) - plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + #plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + plot(t,input[1:j,1,i],t,input[1:j,2,i]) + plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*") legend(["a_x","d_f"]) title("Comparison between velocity and inputs in lap $i, iteration $j ") @@ -538,7 +538,9 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool) legend(["e_y"]) subplot(122) - plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + # plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + plot(t,input[1:j,1,i],t,input[1:j,2,i]) + plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*") legend(["a_x","d_f"]) title("Comparison between e_y and inputs in lap $i, iteration $j ") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 697ccc94..714dbea9 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -134,6 +134,7 @@ function main() log_step_diff = zeros(10000,5) log_t_solv = zeros(10000) log_sol_status = Array(Symbol,10000) + log_final_counter = zeros(30) selStates_log = zeros(selectedStates.Nl*selectedStates.Np,6,buffersize,30) #array to log the selected states in every iteration of every lap statesCost_log = zeros(selectedStates.Nl*selectedStates.Np,buffersize,30) #array to log the selected states' costs in every iteration of every lap @@ -275,6 +276,8 @@ function main() println("Finishing one lap at iteration ",i) # Important: lapStatus.currentIt is now the number of points up to s > s_target -> -1 in saveOldTraj + log_final_counter[lapStatus.currentLap-1] = k + oldSS.oldCost[lapStatus.currentLap-1] = lapStatus.currentIt cost2target = zeros(buffersize) # array containing the cost to arrive from each point of the old trajectory to the target #save the terminal cost @@ -516,7 +519,7 @@ function main() log_mpcCostSlack[lapStatus.currentIt,:,lapStatus.currentLap]= mpcSol.costSlack end - #log_status[lapStatus.currentIt,lapStatus.currentLap] = mpcSol.solverStatus + log_status[lapStatus.currentIt,lapStatus.currentLap] = mpcSol.solverStatus k = k + 1 # counter log_sol_status[k] = mpcSol.solverStatus @@ -565,8 +568,7 @@ function main() "t_solv",log_t_solv[1:k],"sol_status",log_sol_status[1:k],"selectedStates",selectedStates,"one_step_error",log_onestep, "oldSS",oldSS,"selStates",selStates_log,"statesCost",statesCost_log,"pred_sol",log_predicted_sol,"pred_input",log_predicted_input,"lapStatus",lapStatus, "posInfo",posInfo,"eps_alpha",log_epsalpha,"cvx",log_cvx,"cvy",log_cvy,"cpsi",log_cpsi,"oldSS_xy",oldSS.oldSS_xy,"input",log_input, - "mpcCost",log_mpcCost,"mpcCostSlack",log_mpcCostSlack,"obs_log",log_obs) - #,"status",log_status) + "mpcCost",log_mpcCost,"mpcCostSlack",log_mpcCostSlack,"obs_log",log_obs,"final_counter",log_final_counter[1:lapStatus.currentLap])#,"status",log_status) println("Exiting LMPC node. Saved data to $log_path.") println("number of same s in path following = ",same_sPF) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 19f02478..c022702e 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,7 +46,7 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use + selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use if selectedStates.simulator == false # if the BARC is in use @@ -155,11 +155,11 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem - obstacle.lap_active = 100 # number of the first lap in which the obstacles are used + obstacle.lap_active = 100000 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [19] # initial s coordinate of each obstacle - obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle + obstacle.s_obs_init = [16] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [-0.4] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.2#0.5 obstacle.r_ey = 0.1#0.2 From 8ea3ac3cd9f08c5a33826790a50bb57748553689 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 8 Feb 2018 13:57:10 -0800 Subject: [PATCH 151/183] new tuning for experiments --- eval_sim/eval_data.jl | 16 +++++++------- workspace/src/barc/launch/imu-remote.launch | 22 +++++++++++++++---- .../src/barc/launch/sensor_test_remote.launch | 4 ++-- workspace/src/barc/src/LMPC_node.jl | 2 ++ .../src/barc/src/Localization_helpers.py | 2 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 8 +++---- .../src/barc/src/barc_lib/LMPC/functions.jl | 16 +++++++------- .../state_estimation_SensorKinematicModel.py | 2 +- 8 files changed, 44 insertions(+), 28 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index db32d60c..f5c78ada 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -450,15 +450,15 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst #for i=1:4:size(x_est,1) # for index=1:length(oldSS_xy[:,1,i]) - z_pred = zeros(size(pred_sol)[1],4) - #z_pred[1,:] = x_est[i,:] - z_pred[1,:] = oldSS_xy[j,:,i] - for j2=2:size(pred_sol)[1] - z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) - end - plot(z_pred[:,1],z_pred[:,2],"-+") + # z_pred = zeros(size(pred_sol)[1],4) + # #z_pred[1,:] = x_est[i,:] + # z_pred[1,:] = oldSS_xy[j,:,i] + # for j2=2:size(pred_sol)[1] + # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) + # end + plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") grid("on") - title("Predicted solution in lap $i, iteration $j") + # title("Predicted solution in lap $i, iteration $j") # end diff --git a/workspace/src/barc/launch/imu-remote.launch b/workspace/src/barc/launch/imu-remote.launch index fc91c91d..fa53cbf5 100644 --- a/workspace/src/barc/launch/imu-remote.launch +++ b/workspace/src/barc/launch/imu-remote.launch @@ -1,12 +1,26 @@ - - + + + - + + + + + - + + + + + + + + + + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch index 7dd6fbd2..2cc98c4d 100644 --- a/workspace/src/barc/launch/sensor_test_remote.launch +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -7,8 +7,8 @@ - - + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 714dbea9..81f7c2bf 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -547,6 +547,8 @@ function main() #log_epsalpha[:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.eps_alpha end + println("solution u= ",mpcSol.u) + # Count one up: lapStatus.currentIt += 1 else diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index bfc5ea13..09830b14 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -39,7 +39,7 @@ class Localization(object): pos = 0 # current position psi = 0 # current orientation nodes = array([0]) # all nodes are saved in a matrix - N_nodes_poly_back = 30 # number of nodes behind current position + N_nodes_poly_back = 20 # number of nodes behind current position N_nodes_poly_front = 100 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index 152a104f..e875a4db 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -540,11 +540,11 @@ type MpcModel_convhull # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) # end - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.12) - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.12) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.12) - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.12) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) end diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index c022702e..9e9e82d6 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,38 +46,38 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use + selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use if selectedStates.simulator == false # if the BARC is in use - selectedStates.Np = 23 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.shift = 7 + selectedStates.Np = 25 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.shift = 8 Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - - mpcParams.N = 12 + mpcParams.N = 14 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,1.0] #a_x,delta_f # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 2 # scaling of Q-function + mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 3 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 1*[20.0,20.0,10.0,30.0,80.0,1*50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories elseif selectedStates.simulator == true # if the simulator is in use selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.shift = 3 Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index f3cd89c0..b72d90dc 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -224,7 +224,7 @@ def state_estimation(): Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) - #R = diag([10.0,10.0,1.0,10.0,100.0,1000.0,1000.0, 10.0,10.0,10.0,1.0, 10.0,10.0]) + R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters From ecba94923ebf626643dfa874dd212ae75dc4b467 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Sat, 10 Feb 2018 02:55:37 -0800 Subject: [PATCH 152/183] new tuning for simulation --- env_loader_pc.sh | 4 +- eval_sim/eval_data.jl | 107 +++++++++++++----- workspace/src/barc/launch/barc_sim.launch | 4 + .../src/barc/launch/run_lmpc_remote.launch | 4 +- workspace/src/barc/src/LMPC_node.jl | 1 - .../src/barc/src/Localization_helpers.py | 2 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 3 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 20 ++-- 8 files changed, 98 insertions(+), 47 deletions(-) diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 139f04b3..ddf2680d 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -2,6 +2,6 @@ source /opt/ros/kinetic/setup.bash source /home/mpcubuntu/barc/workspace/devel/setup.bash #export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=10.0.0.14 -export ROS_MASTER_URI=http://10.0.0.14:11311 +export ROS_IP=192.168.10.147 +export ROS_MASTER_URI=http://192.168.10.147:11311 exec "$@" diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index f5c78ada..7cc19ef6 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -188,7 +188,8 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst for i = laps - pred_sol_xy = xyObstacle(oldSS,obs_log,1,i,track) + pred_sol_xy, xyPathAngle = xyObstacle(oldSS,obs_log,1,i,track) + #println("pred sol= ",pred_sol_xy[:,1]) # for index=1:buffersize # if status[index,i] == :UserLimit @@ -237,7 +238,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, 0)#angle=angle_deg) + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.2, 0.1, angle = 90) ax[:add_artist](ell1) end @@ -442,25 +443,64 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst t = linspace(1,j,j) - figure(19) - clf() - plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") - #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") - plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - - #for i=1:4:size(x_est,1) - # for index=1:length(oldSS_xy[:,1,i]) - # z_pred = zeros(size(pred_sol)[1],4) - # #z_pred[1,:] = x_est[i,:] - # z_pred[1,:] = oldSS_xy[j,:,i] - # for j2=2:size(pred_sol)[1] - # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) - # end - plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") - grid("on") - # title("Predicted solution in lap $i, iteration $j") - # end + if obstacle == false + figure(19) + clf() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + #for i=1:4:size(x_est,1) + # for index=1:length(oldSS_xy[:,1,i]) + # z_pred = zeros(size(pred_sol)[1],4) + # #z_pred[1,:] = x_est[i,:] + # z_pred[1,:] = oldSS_xy[j,:,i] + # for j2=2:size(pred_sol)[1] + # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) + # end + + + plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + grid("on") + # title("Predicted solution in lap $i, iteration $j") + title("Position at iteration $j") + # end + elseif obstacle == true + ellfig = figure(19) + clf() + # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + #for i=1:4:size(x_est,1) + # for index=1:length(oldSS_xy[:,1,i]) + # z_pred = zeros(size(pred_sol)[1],4) + # #z_pred[1,:] = x_est[i,:] + # z_pred[1,:] = oldSS_xy[j,:,i] + # for j2=2:size(pred_sol)[1] + # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) + # end + + + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,j]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j]-(pred_sol_xy[1,j-1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.2, 0.1, angle = 0)#angle = angle_deg) + ax[:add_artist](ell1) + + plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + grid("on") + # title("Predicted solution in lap $i, iteration $j") + title("Position at iteration $j") + # end + end figure(15) clf() @@ -1728,16 +1768,17 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra # println("obs= ",obs) - OrderXY = 18 - OrderThetaCurv = 12 + OrderXY = 10 + OrderThetaCurv = 8 - ds = 0.0625 + ds = 0.06 s_vec = zeros(OrderXY+1) pred_sol_xy = zeros(2,buffersize,1) + xyPathAngle = [] x_track = track[:,1] @@ -1752,14 +1793,18 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra nodes = [x_track'; y_track'] n_nodes = size(x_track)[1] - s_start = (obs[i,1] - 1) - s_end = (obs[i,1] + 6) + #println("number of nodes= ",n_nodes) + s_start = (obs[i,1] - 2) + #println("s_start= ",s_start) + s_end = (obs[i,1] + 10) + #println("s_end= ",s_end) s_nearest = obs[i,1] - idx_start = 16*(floor(obs[i,1]) - 1) - idx_end = 16*(floor(obs[i,1]) + 6) + idx_start = round(s_start / ds) + #println("idx_start= ",idx_start) + idx_end = round(s_end / ds) + #println("idx_end= ",idx_end) - n_poly = 113 # if idx_start>n_nodes # idx_start=idx_start%n_nodes @@ -1783,6 +1828,10 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra nodes_X = vec(nodes_XY[1,:]) nodes_Y = vec(nodes_XY[2,:]) + #println("length node xy= ",length(nodes_XY[1,:])) + + n_poly = round(Int,length(nodes_XY[1,:])) + #println("n_poly= ",n_poly) @@ -1874,5 +1923,5 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra end - return pred_sol_xy + return pred_sol_xy, xyPathAngle end diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index 3abaef21..b3ad47bc 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -14,6 +14,10 @@ + + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 7520d18d..69b9f59a 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -7,9 +7,9 @@ - + - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 81f7c2bf..350fc5ff 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -547,7 +547,6 @@ function main() #log_epsalpha[:,lapStatus.currentIt,lapStatus.currentLap] = mpcSol.eps_alpha end - println("solution u= ",mpcSol.u) # Count one up: lapStatus.currentIt += 1 diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py index 09830b14..bfc5ea13 100755 --- a/workspace/src/barc/src/Localization_helpers.py +++ b/workspace/src/barc/src/Localization_helpers.py @@ -39,7 +39,7 @@ class Localization(object): pos = 0 # current position psi = 0 # current orientation nodes = array([0]) # all nodes are saved in a matrix - N_nodes_poly_back = 20 # number of nodes behind current position + N_nodes_poly_back = 30 # number of nodes behind current position N_nodes_poly_front = 100 # number of nodes in front ds = 0 # distance between nodes nPoints = N_nodes_poly_front+N_nodes_poly_back+1 # number of points for interpolation in total diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index e875a4db..b618562a 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -554,7 +554,7 @@ type MpcModel_convhull # Derivative cost # --------------------------------- @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} + - QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ + QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-1})+ QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) # Control Input cost @@ -611,7 +611,6 @@ type MpcModel_convhull @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackVx + Q_slack[2]*slackVy + Q_slack[3]*slackPsidot + Q_slack[4]*slackEpsi + Q_slack[5]*slackEy + Q_slack[6]*slackS) #+ controlCost - sol_stat=solve(mdl) println("Finished solve 1 convhull mpc: $sol_stat") sol_stat=solve(mdl) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 9e9e82d6..06b9fdc7 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,7 +46,7 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use + selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use if selectedStates.simulator == false # if the BARC is in use @@ -77,25 +77,25 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull - selectedStates.shift = 3 + selectedStates.shift = 5 Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 10 + mpcParams.N = 13 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[1.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.QderivU = 1.0*[1,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 1 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_lane = 3 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 1*[20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 1*[20.0,1.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories end @@ -155,11 +155,11 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem - obstacle.lap_active = 100000 # number of the first lap in which the obstacles are used + obstacle.lap_active = 1000 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [16] # initial s coordinate of each obstacle - obstacle.ey_obs_init = [-0.4] # initial ey coordinate of each obstacle + obstacle.s_obs_init = [14] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.2#0.5 obstacle.r_ey = 0.1#0.2 From 149e37b8f242ee99cd9e9a350bea06bd1b731bd7 Mon Sep 17 00:00:00 2001 From: Ugo Date: Sat, 10 Feb 2018 12:01:53 -0800 Subject: [PATCH 153/183] Good Tuning --- env_loader_pc.sh | 6 +++--- workspace/src/barc/launch/run_lmpc_remote.launch | 4 ++-- workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 4 ++-- .../src/barc/src/state_estimation_SensorKinematicModel.py | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/env_loader_pc.sh b/env_loader_pc.sh index ddf2680d..9b8ac9dd 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -1,7 +1,7 @@ #!/bin/bash source /opt/ros/kinetic/setup.bash -source /home/mpcubuntu/barc/workspace/devel/setup.bash +source /home/ugo/GitHub/barc/workspace/devel/setup.bash #export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=192.168.10.147 -export ROS_MASTER_URI=http://192.168.10.147:11311 +export ROS_IP=10.0.0.72 +export ROS_MASTER_URI=http://10.0.0.72:11311 exec "$@" diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch index 69b9f59a..52a86790 100644 --- a/workspace/src/barc/launch/run_lmpc_remote.launch +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -7,9 +7,9 @@ - + - + diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index b618562a..b33c1039 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -461,7 +461,7 @@ type MpcModel_convhull z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_lb_6s = ones(mpcParams.N,1) * [-1.3 -0.3] # lower bounds on steering u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds for i=1:2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 06b9fdc7..e4e90fbe 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,11 +46,11 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use + selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use if selectedStates.simulator == false # if the BARC is in use - selectedStates.Np = 25 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull selectedStates.shift = 8 Nl = selectedStates.Nl diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index b72d90dc..dc2d9867 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -224,7 +224,7 @@ def state_estimation(): Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) - R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) + #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters From 62d954d80f6f53d37cf01390ad0ff9b6646cbe0f Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 12 Feb 2018 14:55:18 -0800 Subject: [PATCH 154/183] added boolean variable for obtacle tuning --- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 56 ++++++++++++++++++- workspace/src/barc/src/barc_lib/classes.jl | 3 +- .../state_estimation_SensorKinematicModel.py | 2 +- 4 files changed, 59 insertions(+), 4 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index b618562a..b33c1039 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -461,7 +461,7 @@ type MpcModel_convhull z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_lb_6s = ones(mpcParams.N,1) * [-1.3 -0.3] # lower bounds on steering u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds for i=1:2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 06b9fdc7..39d161e4 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,10 +47,11 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use + obstacle.obstacle_tuning = false if selectedStates.simulator == false # if the BARC is in use - selectedStates.Np = 25 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull selectedStates.shift = 8 Nl = selectedStates.Nl @@ -73,6 +74,33 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + if obstacle.obstacle_tuning == true + + selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.shift = 8 + Nl = selectedStates.Nl + selectedStates.selStates = zeros(Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(Nl*selectedStates.Np) + selectedStates.version = false + + mpcParams.N = 14 + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 1 # reference speed for first lap of path following + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + end + + elseif selectedStates.simulator == true # if the simulator is in use selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull @@ -97,6 +125,32 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity mpcParams.Q_slack = 1*[20.0,1.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + + if obstacle.obstacle_tuning == true + + selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.shift = 5 + Nl = selectedStates.Nl + selectedStates.selStates = zeros(Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(Nl*selectedStates.Np) + selectedStates.version = false + + mpcParams.N = 13 + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 1 # reference speed for first lap of path following + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[1,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 1 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 3 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity + mpcParams.Q_slack = 1*[20.0,1.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + end end diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index f6d85246..a46e678f 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -140,6 +140,7 @@ type Obstacle r_s::Float64 # radius on the s coordinate of the ellipse describing the obstacles r_ey::Float64 # radius on the ey coordinate of the ellipse describing the obstacle inv_step::Int64 # number of step of invariance required for the safe set + obstacle_tuning::Bool # true if we are using the tuning made for obstacle avoidance - Obstacle(obstacle_active=false,lap_active=10,obs_detect=1.0,n_obs=1,s_obs_init=Float64[],ey_obs_init=Float64[],v_obs_init=Float64[],r_s=0.5,r_ey=0.3,inv_step=1) = new(obstacle_active,lap_active,obs_detect,n_obs,s_obs_init,ey_obs_init,v_obs_init,r_s,r_ey,inv_step) + Obstacle(obstacle_active=false,lap_active=10,obs_detect=1.0,n_obs=1,s_obs_init=Float64[],ey_obs_init=Float64[],v_obs_init=Float64[],r_s=0.5,r_ey=0.3,inv_step=1,obstacle_tuning=false) = new(obstacle_active,lap_active,obs_detect,n_obs,s_obs_init,ey_obs_init,v_obs_init,r_s,r_ey,inv_step,obstacle_tuning) end \ No newline at end of file diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index b72d90dc..dc2d9867 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -224,7 +224,7 @@ def state_estimation(): Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) - R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) + #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters From 605e74e6c30609c8b24428d471697a741b410029 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Mon, 12 Feb 2018 18:10:57 -0800 Subject: [PATCH 155/183] added plots for obstacle avoidance --- eval_sim/eval_data.jl | 16 ++++++++++++---- workspace/src/barc/src/LMPC_node.jl | 12 +++++++++--- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 2 +- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 10 +++------- .../src/barc/src/barc_lib/LMPC/functions.jl | 16 ++++++++-------- 5 files changed, 33 insertions(+), 23 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 7cc19ef6..29408439 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -238,7 +238,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.2, 0.1, angle = 90) + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 90) ax[:add_artist](ell1) end @@ -489,10 +489,10 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - angle_ell = atan2(pred_sol_xy[2,j]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j]-(pred_sol_xy[1,j-1])) + angle_ell = atan2(pred_sol_xy[2,j+1]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j+1]-(pred_sol_xy[1,j-1])) angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.2, 0.1, angle = 0)#angle = angle_deg) + ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.4, 0.2, angle = angle_deg) ax[:add_artist](ell1) plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") @@ -548,7 +548,15 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst plot(olds,oldeY,"b") plot(olds2,oldeY2,"b") #plot(olds3,oldeY3,"b") - #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) + + if obstacle == true + for obs_ind = 1:size(obs_log)[3] + if obs_log[j,1,obs_ind,i] < max(olds[end],olds2[end]) + plot(obs_log[j,1,obs_ind,i],obs_log[j,2,obs_ind,i],"ko") + end + end + end + title("State eY in lap $i, iteration $j, status $solution_status ") grid("on") diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 350fc5ff..84d49125 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -419,13 +419,19 @@ function main() index1=find(obs_curr[lapStatus.currentIt,1,:].< obstacle.obs_detect+posInfo.s-posInfo.s_target) # look for obstacles that could cause problems - obs_temp[1,1,index1] = posInfo.s_target + obs_curr[lapStatus.currentIt,1,index1] - + if size(index1)[1] > 0 # if at least one obstacle has been identified + obs_temp[1,1,index1] = posInfo.s_target + obs_curr[lapStatus.currentIt,1,index1] # increase the s of s_target so that the algorithm identifies them + end end - dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) + + dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) # find the closest obstacle using the equation of the ellipse. Closest in terms of s and e_y!! obs_near = obs_temp[1,:,index] + + # println("current s= ",posInfo.s) + # println("closest obstacle= ",obs_near[1,1,1]) + # println("distance= ",dist) end # Find coefficients for cost and constraints diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index b33c1039..e155b80d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -1140,7 +1140,7 @@ type MpcModel_obstacle # Soft Constraint on the Obstacle # -------------------------------- - @NLexpression(mdl, obstacleSlackCost, 0.1*sum{-log(((z_Ol[i,6]-obs[i,1])/r_s)^2 + ((z_Ol[i,5]-obs[i,2])/r_ey)^2 -1),i=1:N+1}) + @NLexpression(mdl, obstacleSlackCost, 1*sum{-log(((z_Ol[i,6]-obs[i,1])/r_s)^2 + ((z_Ol[i,5]-obs[i,2])/r_ey)^2 -1),i=1:N+1}) # Velocity Cost diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index 07afdb79..059b3ee6 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -18,11 +18,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams,lapStatus::LapStatus, selectedStates::SelectedStates,oldSS::SafeSetData,obs::Array{Float64},obstacle::Obstacle) - # this computes the coefficients for the cost and constraints - - # Outputs: - # coeffConst - # coeffCost + # Read Inputs s = posInfo.s @@ -124,8 +120,8 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo #println("flag**************************************************************************************************************") index = find(ellipse_check.<=1) # find all the states in the ellipse - mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] = 10 # and set the values of the weight to 10, so that they are excluded from optimization - println("Q_obs= ",mpcParams.Q_obs) + mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] = 100 # and set the values of the weight to 100, so that they are excluded from optimization + #println("Q_obs= ",mpcParams.Q_obs) end end end diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 39d161e4..7fe6a290 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,7 +47,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use - obstacle.obstacle_tuning = false + obstacle.obstacle_tuning = true # set this to TRUE if the tuning for obstacle avoidance is needed, FALSE if not needed if selectedStates.simulator == false # if the BARC is in use @@ -128,9 +128,9 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track if obstacle.obstacle_tuning == true - selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Np = 16 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull - selectedStates.shift = 5 + selectedStates.shift = 8 Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) @@ -208,15 +208,15 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentLap = 1 # initialize lap number lapStatus.currentIt = 0 # current iteration in lap - obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem - obstacle.lap_active = 1000 # number of the first lap in which the obstacles are used + obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) + obstacle.lap_active = 4 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [14] # initial s coordinate of each obstacle + obstacle.s_obs_init = [15] # initial s coordinate of each obstacle obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles - obstacle.r_s = 0.2#0.5 - obstacle.r_ey = 0.1#0.2 + obstacle.r_s = 0.4#0.5 + obstacle.r_ey = 0.2#0.2 obstacle.inv_step = 0 # number of step of invariance required for the safe set end From 34fe3743c920a8314b09060ec60ef06f740ae0a4 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Fri, 16 Feb 2018 10:37:07 -0800 Subject: [PATCH 156/183] cleaning code from useless lines. CHECK TUNING BEFORE MERGING WITH BASE --- eval_sim/eval_data.jl | 249 ++++++++++++++--- workspace/src/barc/src/LMPC_node.jl | 8 +- .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 250 ------------------ .../src/barc_lib/LMPC/coeffConstraintCost.jl | 3 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 3 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 40 --- workspace/src/barc/src/barc_lib/classes.jl | 3 +- 7 files changed, 226 insertions(+), 330 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 29408439..0804c779 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -443,27 +443,30 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst t = linspace(1,j,j) + s_ey_pred = zeros(pred_horizon+1,2) + + s_ey_pred[:,1] = pred_sol[:,6,j,i] + s_ey_pred[:,2] = pred_sol[:,5,j,i] + + #println(s_ey_pred) + + + xy_predictions = xyPrediction(oldSS,s_ey_pred,track) + + #println("xyPredictions= ",xy_predictions) + + if obstacle == false figure(19) clf() - plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i]) + plot(xy_predictions[1,:]',xy_predictions[2,:]',"*") #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") - - #for i=1:4:size(x_est,1) - # for index=1:length(oldSS_xy[:,1,i]) - # z_pred = zeros(size(pred_sol)[1],4) - # #z_pred[1,:] = x_est[i,:] - # z_pred[1,:] = oldSS_xy[j,:,i] - # for j2=2:size(pred_sol)[1] - # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) - # end - - - plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + #plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") grid("on") # title("Predicted solution in lap $i, iteration $j") - title("Position at iteration $j") + title("Prediction at iteration $j, status = $solution_status") # end elseif obstacle == true @@ -557,7 +560,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst end end - title("State eY in lap $i, iteration $j, status $solution_status ") + title("State eY in lap $i, iteration $j, status = $solution_status ") grid("on") figure(17) @@ -611,7 +614,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst - sleep(1) + sleep(0.5) end end end @@ -809,7 +812,7 @@ function plot_v_over_xy(code::AbstractString,lap::Int64) figure() plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=0.5,vmax=2.0) + scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=0.5,vmax=3.0) grid(1) title("x-y-view") axis("equal") @@ -1555,7 +1558,7 @@ function create_track(w) y_l = [w] x_r = [0.0] # starting point y_r = [-w] - ds = 0.06 + ds = 0.03 theta = [0.0] @@ -1595,17 +1598,29 @@ function create_track(w) # SIMPLE GOGGLE TRACK - add_curve(theta,30,0) - add_curve(theta,40,-pi/2) - add_curve(theta,10,0) - add_curve(theta,40,-pi/2) - add_curve(theta,20,pi/10) - add_curve(theta,30,-pi/5) - add_curve(theta,20,pi/10) - add_curve(theta,40,-pi/2) - add_curve(theta,10,0) - add_curve(theta,40,-pi/2) - add_curve(theta,35,0) + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,pi/10) + # add_curve(theta,30,-pi/5) + # add_curve(theta,20,pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + add_curve(theta,60,0) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,40,pi/10) + add_curve(theta,60,-pi/5) + add_curve(theta,40,pi/10) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,75,0) # SIMPLE GOOGLE TRACK FOR 3110 @@ -1781,7 +1796,7 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra - ds = 0.06 + ds = 0.03 s_vec = zeros(OrderXY+1) @@ -1802,9 +1817,9 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra nodes = [x_track'; y_track'] n_nodes = size(x_track)[1] #println("number of nodes= ",n_nodes) - s_start = (obs[i,1] - 2) + s_start = (obs[i,1] - 1) #println("s_start= ",s_start) - s_end = (obs[i,1] + 10) + s_end = (obs[i,1] + 5) #println("s_end= ",s_end) s_nearest = obs[i,1] @@ -1933,3 +1948,173 @@ function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,tra return pred_sol_xy, xyPathAngle end + + + + + +function xyPrediction(oldSS,s_ey_pred::Array{Float64},track::Array{Float64}) + + + pred_horizon = size(s_ey_pred)[1] + + + OrderXY = 10 + OrderThetaCurv = 8 + + + + ds = 0.03 + + s_vec = zeros(OrderXY+1) + + pred_sol_xy = zeros(2,pred_horizon,1) + xyPathAngle = [] + + x_track = track[:,1] + + y_track = track[:,2] + + #println("x_track= ",x_track) + # println("nodes= ",size([x_track'; y_track'])) + for i = 1:pred_horizon + + + + + nodes = [x_track'; y_track'] + n_nodes = size(x_track)[1] + #println("number of nodes= ",n_nodes) + s_start = (s_ey_pred[i,1] - 1) + #println("s_start= ",s_start) + s_end = (s_ey_pred[i,1] + 5) + #println("s_end= ",s_end) + s_nearest = s_ey_pred[i,1] + + idx_start = round(s_start / ds) + #println("idx_start= ",idx_start) + idx_end = round(s_end / ds) + #println("idx_end= ",idx_end) + + + # if idx_start>n_nodes + # idx_start=idx_start%n_nodes + # idx_end=idx_end%n_nodes + # end + + + + if idx_start<=0 + nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together + # #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end]) + + #idx_start = n_nodes+idx_start + elseif idx_end>=n_nodes # if the end is behind the finish line + nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes]) # then stack the end and beginning of the lap together + #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes]) + else # if we are somewhere in the middle of the track + nodes_XY = nodes[:,idx_start:idx_end] # then just use the nodes from idx_start to end for interpolation + #nodes_Y = nodes[2,idx_start:idx_end] + end + + + nodes_X = vec(nodes_XY[1,:]) + nodes_Y = vec(nodes_XY[2,:]) + #println("length node xy= ",length(nodes_XY[1,:])) + + n_poly = round(Int,length(nodes_XY[1,:])) + #println("n_poly= ",n_poly) + + + + itp_matrix = zeros(n_poly,OrderXY+1) + + for ind=1:n_poly + for k=0:OrderXY + + itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k + end + end + + itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1) + + for ind=1:n_poly + for k=0:OrderThetaCurv + + itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k + end + end + + # println("size of nodes x= ",size(nodes_X)) + # println("size of itpmatrix= ",size(itp_matrix)) + # println("s start= ",s_start) + # println("s end= ",s_end) + + coeffY = itp_matrix\nodes_Y + coeffX = itp_matrix\nodes_X + + + b_curvature_vector = zeros(n_poly) + + Counter = 1 + + + for ind = 0:n_poly-1 + s_expression_der = zeros(OrderXY+1) + s_expression_2der = zeros(OrderXY+1) + s_poly = s_start + ind*ds + for k=0:OrderXY-1 + s_expression_der[OrderXY-k] = (k+1)*s_poly^k + end + for k=0:OrderXY-2 + s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k + end + + dX = dot(coeffX,s_expression_der) + dY = dot(coeffY,s_expression_der) + ddX = dot(coeffX,s_expression_2der) + ddY = dot(coeffY,s_expression_2der) + + curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula + + b_curvature_vector[Counter] = curvature + + Counter = Counter + 1 + end + + + + coeffCurv = itp_matrix_curv\b_curvature_vector + + s0 = s_ey_pred[i,1]+0.001 + + s_vec = zeros(OrderXY+1)::Array{Float64} + sdot_vec = zeros(OrderXY+1)::Array{Float64} + + for k = 1:OrderXY+1 + s_vec[k] = s_ey_pred[i,1]^(OrderXY-k+1) + + end + for k = 1:OrderXY + sdot_vec[k] = (OrderXY+1-k)* s_ey_pred[i,1]^(OrderXY-k) + end + + + XCurve = dot(coeffX,s_vec) + YCurve = dot(coeffY,s_vec) + + dX = dot(coeffX,sdot_vec) + dY = dot(coeffY,sdot_vec) + + + xyPathAngle = atan2(dY,dX) + + pred_sol_xy[2,i] = YCurve + s_ey_pred[i,2]*cos(xyPathAngle) + pred_sol_xy[1,i] = XCurve - s_ey_pred[i,2]*sin(xyPathAngle) + + + end + + + return pred_sol_xy +end diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 84d49125..585a55d0 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -100,9 +100,6 @@ function main() mdl_convhull = MpcModel_convhull(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) end - - - #mdl_test = MpcModel_test(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates) mdl_obstacle = MpcModel_obstacle(mpcParams,mpcCoeff,modelParams,trackCoeff,selectedStates,obstacle) @@ -290,6 +287,9 @@ function main() if lapStatus.currentLap == obstacle.lap_active # if its time to put the obstacles in the track obstacle.obstacle_active = true # tell the system to put the obstacles on the track end + if lapStatus.currentLap == obstacle.lap_deactivate + obstacle.obstacle_active = false + end if lapStatus.currentLap > obstacle.lap_active # initialize current obstacle states with final states from the previous lap obs_curr[1,:,:] = obs_curr[i,:,:] end @@ -459,9 +459,7 @@ function main() solveMpcProblem(mdl,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev) elseif selectedStates.version == false && obstacle.obstacle_active == false solveMpcProblem_convhull(mdl_convhull,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) - #solveMpcProblem_test(mdl_test,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates) elseif selectedStates.version == false && obstacle.obstacle_active == true - solveMpcProblem_obstacle(mdl_obstacle,mpcSol,mpcCoeff,mpcParams,trackCoeff,lapStatus,posInfo,modelParams,zCurr[i,:]',uPrev,selectedStates,obs_near,obstacle) end diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index e155b80d..1588dd31 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -651,257 +651,7 @@ end -type MpcModel_test - mdl::JuMP.Model - - z0::Array{JuMP.NonlinearParameter,1} - coeff::Array{JuMP.NonlinearParameter,1} - selStates::Array{JuMP.NonlinearParameter,2} - statesCost::Array{JuMP.NonlinearParameter,1} - c_Vx::Array{JuMP.NonlinearParameter,1} - c_Vy::Array{JuMP.NonlinearParameter,1} - c_Psi::Array{JuMP.NonlinearParameter,1} - uPrev::Array{JuMP.NonlinearParameter,2} - - - eps_lane::Array{JuMP.Variable,1} - #eps_alpha::Array{JuMP.Variable,1} - #eps_vel::Array{JuMP.Variable,1} - alpha::Array{JuMP.Variable,1} - z_Ol::Array{JuMP.Variable,2} - u_Ol::Array{JuMP.Variable,2} - - - dsdt::Array{JuMP.NonlinearExpression,1} - c::Array{JuMP.NonlinearExpression,1} - - derivCost::JuMP.NonlinearExpression - controlCost::JuMP.NonlinearExpression - laneCost::JuMP.NonlinearExpression - terminalCost::JuMP.NonlinearExpression - costPF::JuMP.NonlinearExpression - #slackCost::JuMP.NonlinearExpression - #velocityCost::JuMP.NonlinearExpression - - function MpcModel_test(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates) - - - m = new() - - #### Initialize parameters - - dt = modelParams.dt # time step - L_a = modelParams.l_A # distance from CoM of the car to the front wheels - L_b = modelParams.l_B # distance from CoM of the car to the rear wheels - u_lb = modelParams.u_lb # lower bounds for the control inputs - u_ub = modelParams.u_ub # upper bounds for the control inputs - z_lb = modelParams.z_lb # lower bounds for the states - z_ub = modelParams.z_ub # upper bounds for the states - c0 = modelParams.c0 - - - ey_max = trackCoeff.width/2 # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons - n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree for curvature approximation - v_max = 3 # maximum allowed velocity - - v_ref = mpcParams.vPathFollowing # reference velocity for the path following - N = mpcParams.N # Prediction horizon - QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states - QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs - R = mpcParams.R::Array{Float64,1} # weights on the control inputs - Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following - Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane - #Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity - delay_df = mpcParams.delay_df - delay_a = mpcParams.delay_a - - - Np = selectedStates.Np::Int64 # how many states to select - Nl = selectedStates.Nl::Int64 # how many previous laps to select - - acc_f = 1.0 - - # Create function-specific parameters - z_Ref::Array{Float64,2} - z_Ref = cat(2,v_ref*ones(N+1,1),zeros(N+1,5)) # Reference trajectory: path following -> stay on line and keep constant velocity - u_Ref = zeros(N,2) - - - mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) - - @variable( mdl, z_Ol[1:(N+1),1:7]) - @variable( mdl, u_Ol[1:N,1:2]) - @variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints - @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull - # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha - #@variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity - - - - z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states - z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering - u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds - - for i=1:2 - for j=1:N - setlowerbound(u_Ol[j,i], u_lb_6s[j,i]) - setupperbound(u_Ol[j,i], u_ub_6s[j,i]) - end - end - for i=1:7 - for j=1:N+1 - setlowerbound(z_Ol[j,i], z_lb_6s[j,i]) - setupperbound(z_Ol[j,i], z_ub_6s[j,i]) - end - end - - @NLparameter(mdl, z0[i=1:7] == 0) - @NLparameter(mdl, coeff[i=1:n_poly_curv+1] == 0) - @NLparameter(mdl, c_Vx[i=1:3] == 0) - @NLparameter(mdl, c_Vy[i=1:4] == 0) - @NLparameter(mdl, c_Psi[i=1:3] == 0) - @NLparameter(mdl, uPrev[1:10,1:2] == 0) - @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0) # states from the previous trajectories selected in "convhullStates" - @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" - - - # Conditions for first solve: - setvalue(z0[1],1) - setvalue(c_Vx[3],0.1) - - @NLconstraint(mdl, [i=1:7], z_Ol[1,i] == z0[i]) - - @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] <= ey_max + eps_lane[i]) - @NLconstraint(mdl, [i=2:N+1], z_Ol[i,5] >= -ey_max - eps_lane[i]) - #@NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # sof constraint on maximum velocity - @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull - - for i = 1:6 - @NLconstraint(mdl,z_Ol[N+1,i] == sum{alpha[j]*selStates[j,i],j=1:Nl*Np}) # terminal constraint - #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}-eps_alpha[n]) - #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:Nl*Np}+eps_alpha[n]) - end - - @NLexpression(mdl, c[i = 1:N], sum{coeff[j]*z_Ol[i,6]^(n_poly_curv-j+1),j=1:n_poly_curv} + coeff[n_poly_curv+1]) - @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,1]*cos(z_Ol[i,4]) - z_Ol[i,2]*sin(z_Ol[i,4]))/(1-z_Ol[i,5]*c[i])) - - println("Initializing model...") - - # System dynamics - for i=1:N - if i<=delay_df - @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*uPrev[delay_df+1-i,2]) # yDot - @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*uPrev[delay_df+1-i,2]) # psiDot - else - @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + c_Vy[1]*z_Ol[i,2]/z_Ol[i,1] + c_Vy[2]*z_Ol[i,1]*z_Ol[i,3] + c_Vy[3]*z_Ol[i,3]/z_Ol[i,1] + c_Vy[4]*u_Ol[i-delay_df,2]) # yDot - @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + c_Psi[1]*z_Ol[i,3]/z_Ol[i,1] + c_Psi[2]*z_Ol[i,2]/z_Ol[i,1] + c_Psi[3]*u_Ol[i-delay_df,2]) # psiDot - end - if i<=delay_a - #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(uPrev[delay_a+1-i,1] - 0.5*z_Ol[i,1])) - #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot - @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(uPrev[delay_a+1-i,1]-z_Ol[i,7])*acc_f) - else - #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*u_Ol[i,1]) # xDot - @NLconstraint(mdl, z_Ol[i+1,7] == z_Ol[i,7] + dt*(u_Ol[i-delay_a,1]-z_Ol[i,7])*acc_f) - end - #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2] + c_Vx[2]*z_Ol[i,3] + c_Vx[3]*z_Ol[i,1] + c_Vx[4]*z_Ol[i,7]) # xDot - #@NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*(z_Ol[i,7] - 0.5*z_Ol[i,1])) # xDot - @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + c_Vx[1]*z_Ol[i,2]*z_Ol[i,3] + c_Vx[2]*z_Ol[i,1] + c_Vx[3]*z_Ol[i,7]) - @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(z_Ol[i,3]-dsdt[i]*c[i])) # ePsi - @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt*(z_Ol[i,1]*sin(z_Ol[i,4])+z_Ol[i,2]*cos(z_Ol[i,4]))) # eY - @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + dt*dsdt[i] ) # s - end - @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] <= 0.05) - @NLconstraint(mdl, u_Ol[1,1]-uPrev[1,1] >= -0.2) - for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] <= 0.05) - @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) - end - - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) - @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) - for i=1:N-1 # Constraints on u: - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) - @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) - end - - - - # Cost functions - - # Derivative cost - # --------------------------------- - @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:6} + - QderivU[1]*((uPrev[1,1]-u_Ol[1,1])^2+sum{(u_Ol[i,1]-u_Ol[i+1,1])^2,i=1:N-delay_a-1})+ - QderivU[2]*((uPrev[1,2]-u_Ol[1,2])^2+sum{(u_Ol[i,2]-u_Ol[i+1,2])^2,i=1:N-delay_df-1})) - - # Control Input cost - # --------------------------------- - @NLexpression(mdl, controlCost, R[1]*sum{(u_Ol[i,1])^2,i=1:N-delay_a}+ - R[2]*sum{(u_Ol[i,2])^2,i=1:N-delay_df}) - - # Lane cost (soft) - # --------------------------------- - @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) - - # Terminal Cost - # --------------------------------- - @NLexpression(mdl, terminalCost , sum{alpha[i]*statesCost[i], i=1:Nl*Np}) - - # Slack cost (soft) - # --------------------------------- - #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) - - # Velocity Cost - #---------------------------------- - #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) - - # Path Following Cost - #---------------------------------- - - @NLexpression(mdl, costPF, 0.5*sum{Q[i]*sum{(z_Ol[j,i]-z_Ref[j,i])^2,j=2:N+1},i=1:6}) # Follow trajectory - - - # Overall Cost function (objective of the minimization) - # ----------------------------------------------------- - - @NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) - #@NLobjective(mdl, Min, derivCost + controlCost + costPF + terminalCost) - - - sol_stat=solve(mdl) - println("Finished solve 1: $sol_stat") - sol_stat=solve(mdl) - println("Finished solve 2: $sol_stat") - - - m.mdl = mdl - m.z0 = z0 - m.coeff = coeff - m.z_Ol = z_Ol - m.u_Ol = u_Ol - m.c_Vx = c_Vx - m.c_Vy = c_Vy - m.c_Psi = c_Psi - m.uPrev = uPrev - #m.eps_alpha=eps_alpha - - m.derivCost = derivCost - m.controlCost = controlCost - m.costPF = costPF - #m.laneCost = laneCost - m.terminalCost= terminalCost # terminal cost - #m.velocityCost= velocityCost #velocity cost - m.selStates = selStates # selected states - m.statesCost = statesCost # cost of the selected states - m.alpha = alpha # parameters of the convex hull - - - return m - end -end type MpcModel_obstacle diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index 059b3ee6..c1f347be 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -53,7 +53,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo end if lapStatus.currentLap >= 5 - selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2]) # and the best from all previous laps + selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-Nl]) # and the best from all previous laps end # Select the old data @@ -96,6 +96,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo idx_s = findmin(DistS,1)[2] # contains both indices for the closest distances for both oldS !! idx_s2= findmin(DistS2,1)[2] + idx_s2 = idx_s2 + selectedStates.shift # Propagate the obstacle for the prediction horizon diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 7fe6a290..f0d4acd4 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -129,7 +129,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track if obstacle.obstacle_tuning == true selectedStates.Np = 16 # Number of points to take from each previous trajectory to build the convex hull - selectedStates.Nl = 2 # Number of previous laps to include in the convex hull + selectedStates.Nl = 3 # Number of previous laps to include in the convex hull selectedStates.shift = 8 Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) @@ -209,6 +209,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) + obstacle.lap_deactivate = 6 # number of lap in which to stop considering obstacles obstacle.lap_active = 4 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index ca6e92e4..578227bc 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -168,47 +168,7 @@ function solveMpcProblem_convhull(m::MpcModel_convhull,mpcSol::MpcSol,mpcCoeff:: nothing end -function solveMpcProblem_test(m::MpcModel_test,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus, - posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},selectedStates::SelectedStates) - - # Load Parameters - sol_status::Symbol - sol_u::Array{Float64,2} - sol_z::Array{Float64,2} - - selStates = selectedStates.selStates::Array{Float64,2} - statesCost = selectedStates.statesCost::Array{Float64,1} - - # Update current initial condition, curvature and System ID coefficients - setvalue(m.z0,zCurr) - setvalue(m.uPrev,uPrev) - setvalue(m.c_Vx,mpcCoeff.c_Vx) # System ID coefficients - setvalue(m.c_Vy,mpcCoeff.c_Vy) - setvalue(m.c_Psi,mpcCoeff.c_Psi) - setvalue(m.coeff,trackCoeff.coeffCurvature) # Track curvature - setvalue(m.selStates,selStates) - setvalue(m.statesCost,statesCost) - # Solve Problem and return solution - sol_status = solve(m.mdl) - sol_u = getvalue(m.u_Ol) - sol_z = getvalue(m.z_Ol) - - # export data - mpcSol.a_x = sol_u[1,1] - mpcSol.d_f = sol_u[1,2] - mpcSol.u = sol_u - mpcSol.z = sol_z - #mpcSol.eps_alpha = getvalue(m.eps_alpha) - mpcSol.solverStatus = sol_status - mpcSol.cost = zeros(6) - #mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] - #println("o,terminal,control,deriv,0,lane= ",mpcSol.cost) - - println("Solved, status = $sol_status") - - nothing -end function solveMpcProblem_obstacle(m::MpcModel_obstacle,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus, posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},selectedStates::SelectedStates, diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index a46e678f..55f5b582 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -131,6 +131,7 @@ end type Obstacle obstacle_active::Bool # true if we have to consider the obstacles in the optimization problem + lap_deactivate::Int64 # number of the lap in which to stop using obstacles lap_active::Int64 # number of the first lap in which the obstacles are used obs_detect::Float64 # maximum distance at which we can detect obstacles (in terms of s!!) n_obs::Int64 # number of obstacles in the track @@ -142,5 +143,5 @@ type Obstacle inv_step::Int64 # number of step of invariance required for the safe set obstacle_tuning::Bool # true if we are using the tuning made for obstacle avoidance - Obstacle(obstacle_active=false,lap_active=10,obs_detect=1.0,n_obs=1,s_obs_init=Float64[],ey_obs_init=Float64[],v_obs_init=Float64[],r_s=0.5,r_ey=0.3,inv_step=1,obstacle_tuning=false) = new(obstacle_active,lap_active,obs_detect,n_obs,s_obs_init,ey_obs_init,v_obs_init,r_s,r_ey,inv_step,obstacle_tuning) + Obstacle(obstacle_active=false,lap_deactivate=12,lap_active=10,obs_detect=1.0,n_obs=1,s_obs_init=Float64[],ey_obs_init=Float64[],v_obs_init=Float64[],r_s=0.5,r_ey=0.3,inv_step=1,obstacle_tuning=false) = new(obstacle_active,lap_deactivate,lap_active,obs_detect,n_obs,s_obs_init,ey_obs_init,v_obs_init,r_s,r_ey,inv_step,obstacle_tuning) end \ No newline at end of file From db7e652d3a5b3ce232dfaa2240912c6f0b817245 Mon Sep 17 00:00:00 2001 From: Ugo Date: Fri, 16 Feb 2018 12:26:33 -0800 Subject: [PATCH 157/183] Latest Tuning --- "\033OA" | 35 +++++++++++++ c | 51 +++++++++++++++++++ .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 4 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 22 ++++---- 4 files changed, 99 insertions(+), 13 deletions(-) create mode 100644 "\033OA" create mode 100644 c diff --git "a/\033OA" "b/\033OA" new file mode 100644 index 00000000..05d9f14c --- /dev/null +++ "b/\033OA" @@ -0,0 +1,35 @@ +diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +index e4e90fb..2e7a0a8 100644 +--- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl ++++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +@@ -64,13 +64,13 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states +- mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs ++ mpcParams.QderivU = 1.0*[10.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity +- mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s ++ mpcParams.Q_slack = 1*[5*20.0,1*20.0,1*10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories +  + elseif selectedStates.simulator == true # if the simulator is in use +diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +index dc2d986..93c7f26 100755 +--- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py ++++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +@@ -224,8 +224,8 @@ def state_estimation(): +  + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) +- #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) +- # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v ++ #R = diag([4*5.0,4*5.0,1.0,10.0,2*100.0,2*1000.0,2*1000.0, 4*5.0,4*5.0,10.0,1.0, 2*10.0,10.0]) ++ # x, y, v, psi,psiDot,a_x,a_y, x, y, psi, v +  + # Set up track parameters + l = Localization() diff --git a/c b/c new file mode 100644 index 00000000..bfbf9620 --- /dev/null +++ b/c @@ -0,0 +1,51 @@ +diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +index b33c103..be58230 100644 +--- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl ++++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +@@ -540,11 +540,11 @@ type MpcModel_convhull + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end +  +- @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) +- @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) ++ @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.08) ++ @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.08) + for i=1:N-1 # Constraints on u: +- @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) +- @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) ++ @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.08) ++ @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.08) + end +  +  +diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +index e4e90fb..ae130af 100644 +--- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl ++++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +@@ -68,9 +68,9 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track + mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay +- mpcParams.Q_lane = 1 # weight on the soft constraint for the lane ++ mpcParams.Q_lane = 2 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity +- mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s ++ mpcParams.Q_slack = 1*[5*20.0,1*20.0,1*10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories +  + elseif selectedStates.simulator == true # if the simulator is in use +diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +index dc2d986..18c8ad9 100755 +--- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py ++++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +@@ -224,8 +224,8 @@ def state_estimation(): +  + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) +- #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) +- # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v ++ R = diag([4*5.0,4*5.0,1.0,10.0,2*100.0,2*1000.0,2*1000.0, 4*5.0,4*5.0,10.0,1.0, 2*10.0,10.0]) ++ # x, y, v, psi,psiDot,a_x,a_y, x, y, psi, v +  + # Set up track parameters + l = Localization() diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl index e155b80d..8e8f7396 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -564,7 +564,7 @@ type MpcModel_convhull # Lane cost (soft) # --------------------------------- - @NLexpression(mdl, laneCost, Q_lane*sum{40.0*eps_lane[i]+300.0*eps_lane[i]^2 ,i=2:N+1}) + @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+100.0*eps_lane[i]^2 ,i=2:N+1}) # Terminal Cost @@ -999,7 +999,7 @@ type MpcModel_obstacle z_lb_6s = ones(mpcParams.N+1,1)*[0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states z_ub_6s = ones(mpcParams.N+1,1)*[3.5 Inf Inf Inf Inf Inf Inf] # upper bounds - u_lb_6s = ones(mpcParams.N,1) * [-1.0 -0.3] # lower bounds on steering + u_lb_6s = ones(mpcParams.N,1) * [-1.3 -0.3] # lower bounds on steering u_ub_6s = ones(mpcParams.N,1) * [2.0 0.3] # upper bounds for i=1:2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 7fe6a290..f90906a6 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -46,7 +46,7 @@ end function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,modelParams::ModelParams, posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) - selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use + selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use obstacle.obstacle_tuning = true # set this to TRUE if the tuning for obstacle avoidance is needed, FALSE if not needed if selectedStates.simulator == false # if the BARC is in use @@ -59,19 +59,19 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 14 + mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.QderivU = 1.0*[5.0,0.1] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 5 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 1*[5*20.0,0.5*20.0,1*10.0,30.0,0.1*80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories if obstacle.obstacle_tuning == true @@ -84,19 +84,19 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 14 + mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.QderivU = 1.0*[5.0,0.1] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 5 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_slack = 1*[5*20.0,0.5*20.0,1*10.0,30.0,0.1*80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories end @@ -209,10 +209,10 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) - obstacle.lap_active = 4 # number of the first lap in which the obstacles are used + obstacle.lap_active = 6 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [15] # initial s coordinate of each obstacle + obstacle.s_obs_init = [18] # initial s coordinate of each obstacle obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.4#0.5 From 4c1113831efb9976a2d557d56b06d0a77d62e67b Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Fri, 16 Feb 2018 16:17:29 -0800 Subject: [PATCH 158/183] added one lap for system ID --- workspace/src/barc/src/LMPC_node.jl | 2 +- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 105 ++++++++++++------ .../src/barc/src/barc_lib/LMPC/functions.jl | 18 +-- workspace/src/barc/src/barc_lib/classes.jl | 6 +- 4 files changed, 91 insertions(+), 40 deletions(-) diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 585a55d0..9494ca30 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -210,7 +210,7 @@ function main() uPrev = zeros(10,2) # saves the last 10 inputs (1 being the most recent one) - n_pf = 3 # number of first path-following laps (needs to be at least 2) + n_pf = 4 # number of first path-following laps (needs to be at least 2) acc0 = 0.0 opt_count = 0 diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index c1f347be..ef3897a8 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -29,6 +29,10 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo Np = selectedStates.Np Nl = selectedStates.Nl + Nl_sID = selectedStates.Nl_sID + lambda1 = selectedStates.lambda1 + lambda2 = selectedStates.lambda2 + lambda3 = selectedStates.lambda3 # Parameters Order = mpcCoeff.order # interpolation order for cost and constraints @@ -194,7 +198,31 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo cC = oldTraj.count[lapStatus.currentLap]-1 # current count cL = lapStatus.currentLap # current lap #vec_range_ID2 = cC-n_prev:cC-1 # index range for current lap - + + selected_laps_sID = zeros(Int64,Nl_sID) + + for i = 1:Nl_sID + selected_laps_sID[i] = lapStatus.currentLap-i # use previous lap + end + + oldxDot_sID = oldTraj.oldTraj[:,1,selected_laps_sID]::Array{Float64,3} + oldyDot_sID = oldTraj.oldTraj[:,2,selected_laps_sID]::Array{Float64,3} + oldpsiDot_sID = oldTraj.oldTraj[:,3,selected_laps_sID]::Array{Float64,3} + oldePsi_sID = oldTraj.oldTraj[:,4,selected_laps_sID]::Array{Float64,3} + oldeY_sID = oldTraj.oldTraj[:,5,selected_laps_sID]::Array{Float64,3} + oldS_sID = oldTraj.oldTraj[:,6,selected_laps_sID]::Array{Float64,3} + oldacc_sID = oldTraj.oldTraj[:,7,selected_laps_sID]::Array{Float64,3} + olda_sID = oldTraj.oldInput[:,1,selected_laps_sID]::Array{Float64,3} + olddF_sID = oldTraj.oldInput[:,2,selected_laps_sID]::Array{Float64,3} + + + oldS_sID = oldTraj.oldTraj[:,6,selected_laps_sID]::Array{Float64,3} + + DistS_sID = ( s_total - oldS_sID ).^2 + + + idx_s_sID = findmin(DistS_sID,1)[2] + # TODO:******************* # CHECK IF DIFFERENCES ARE ALIGNED PROPERLY # ADD 2-step-delay to sysID @@ -210,54 +238,69 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo # vec_range_ID = tuple(vec_range_ID...,idx_s[i]-n_prev:idx_s[i]+n_ahead) # related index range # end - sysID_idx_diff = idx_s[1]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences + sysID_idx_diff = idx_s_sID[1]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences sysID_idx = sysID_idx_diff[1:end-1] - sysID_idx_diff2 = idx_s[2]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences + sysID_idx_diff2 = idx_s_sID[2]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences sysID_idx2 = sysID_idx_diff2[1:end-1] + sysID_idx_diff3 = idx_s_sID[3]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio) # these are the indices that are used for differences + sysID_idx3 = sysID_idx_diff3[1:end-1] + sysID_idx_diff_c = cC - (n_sys_ID_prev_c+1)*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev_c+1)*freq_ratio) sysID_idx_c = sysID_idx_diff_c[1:end-1] sz1 = size(sysID_idx,1) sz2 = size(sysID_idx_c,1) - sz2 = 0 + #sz2 = 0 sz = sz1 + sz2 # psiDot - y_psi = zeros((2*sz1+sz2)*freq_ratio) - A_psi = zeros((2*sz1+sz2)*freq_ratio,3) + y_psi = zeros((Nl_sID*sz1+sz2)*freq_ratio) + A_psi = zeros((Nl_sID*sz1+sz2)*freq_ratio,3) for i=0:freq_ratio-1 - y_psi[(1:sz1)+i*sz1] = diff(oldpsiDot[sysID_idx_diff+i]) - A_psi[(1:sz1)+i*sz1,:] = [oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i]] - #y_psi[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) - #A_psi[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2,cL]] - y_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2] = diff(oldpsiDot[sysID_idx_diff2+i]) - A_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2,:] = [oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i]] + y_psi[(1:sz1)+i*sz1] = diff(oldpsiDot_sID[sysID_idx_diff+i]) + A_psi[(1:sz1)+i*sz1,:] = [oldpsiDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] oldyDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] olddF_sID[sysID_idx+i]] + y_psi[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) + A_psi[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2,cL]] + y_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2] = diff(oldpsiDot_sID[sysID_idx_diff2+i]) + A_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2,:] = [oldpsiDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] oldyDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] olddF_sID[sysID_idx2+i]] + + y_psi[(1:sz1)+i*sz1+2*freq_ratio*sz1+freq_ratio*sz2] = diff(oldpsiDot_sID[sysID_idx_diff3+i]) + A_psi[(1:sz1)+i*sz1+2*freq_ratio*sz1+freq_ratio*sz2,:] = [oldpsiDot_sID[sysID_idx3+i]./oldxDot_sID[sysID_idx3+i] oldyDot_sID[sysID_idx3+i]./oldxDot_sID[sysID_idx3+i] olddF_sID[sysID_idx3+i]] + end # xDot - y_xDot = zeros((2*sz1+sz2)*freq_ratio) - A_xDot = zeros((2*sz1+sz2)*freq_ratio,3) + y_xDot = zeros((Nl_sID*sz1+sz2)*freq_ratio) + A_xDot = zeros((Nl_sID*sz1+sz2)*freq_ratio,3) for i=0:freq_ratio-1 - y_xDot[(1:sz1)+i*sz1] = diff(oldxDot[sysID_idx_diff+i]) - A_xDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i].*oldpsiDot[sysID_idx+i] oldxDot[sysID_idx+i] oldacc[sysID_idx+i]] - #y_xDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) - #A_xDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] - y_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldxDot[sysID_idx_diff2+i]) - A_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot[sysID_idx2+i].*oldpsiDot[sysID_idx2+i] oldxDot[sysID_idx2+i] oldacc[sysID_idx2+i]] + y_xDot[(1:sz1)+i*sz1] = diff(oldxDot_sID[sysID_idx_diff+i]) + A_xDot[(1:sz1)+i*sz1,:] = [oldyDot_sID[sysID_idx+i].*oldpsiDot_sID[sysID_idx+i] oldxDot_sID[sysID_idx+i] oldacc_sID[sysID_idx+i]] + y_xDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) + A_xDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] + y_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldxDot_sID[sysID_idx_diff2+i]) + A_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot_sID[sysID_idx2+i].*oldpsiDot_sID[sysID_idx2+i] oldxDot_sID[sysID_idx2+i] oldacc_sID[sysID_idx2+i]] + + y_xDot[(1:sz1)+i*sz1+freq_ratio*(2*sz1+sz2)] = diff(oldxDot_sID[sysID_idx_diff3+i]) + A_xDot[(1:sz1)+i*sz1+freq_ratio*(2*sz1+sz2),:] = [oldyDot_sID[sysID_idx3+i].*oldpsiDot_sID[sysID_idx3+i] oldxDot_sID[sysID_idx3+i] oldacc_sID[sysID_idx3+i]] + end # yDot - y_yDot = zeros((2*sz1+sz2)*freq_ratio) - A_yDot = zeros((2*sz1+sz2)*freq_ratio,4) + y_yDot = zeros((Nl_sID*sz1+sz2)*freq_ratio) + A_yDot = zeros((Nl_sID*sz1+sz2)*freq_ratio,4) for i=0:freq_ratio-1 - y_yDot[(1:sz1)+i*sz1] = diff(oldyDot[sysID_idx_diff+i]) - A_yDot[(1:sz1)+i*sz1,:] = [oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i].*oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i-delay_df*freq_ratio]] - #y_yDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) - #A_yDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2]] - y_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldyDot[sysID_idx_diff2+i]) - A_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i].*oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i-delay_df*5]] + y_yDot[(1:sz1)+i*sz1] = diff(oldyDot_sID[sysID_idx_diff+i]) + A_yDot[(1:sz1)+i*sz1,:] = [oldyDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] oldpsiDot_sID[sysID_idx+i].*oldxDot_sID[sysID_idx+i] oldpsiDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] olddF_sID[sysID_idx+i-delay_df*freq_ratio]] + y_yDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) + A_yDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2]] + y_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldyDot_sID[sysID_idx_diff2+i]) + A_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] oldpsiDot_sID[sysID_idx2+i].*oldxDot_sID[sysID_idx2+i] oldpsiDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] olddF_sID[sysID_idx2+i-delay_df*5]] + + y_yDot[(1:sz1)+i*sz1+freq_ratio*(2*sz1+sz2)] = diff(oldyDot_sID[sysID_idx_diff3+i]) + A_yDot[(1:sz1)+i*sz1+freq_ratio*(2*sz1+sz2),:] = [oldyDot_sID[sysID_idx3+i]./oldxDot_sID[sysID_idx3+i] oldpsiDot_sID[sysID_idx3+i].*oldxDot_sID[sysID_idx3+i] oldpsiDot_sID[sysID_idx3+i]./oldxDot_sID[sysID_idx3+i] olddF_sID[sysID_idx3+i-delay_df*5]] + end # if any(isnan,y_yDot) # check if any value in the y_yDot value is NaN @@ -277,9 +320,9 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo # mpcCoeff.c_Vx = zeros(4) # mpcCoeff.c_Vy = zeros(4) - mpcCoeff.c_Psi = (A_psi'*A_psi)\A_psi'*y_psi - mpcCoeff.c_Vx = (A_xDot'*A_xDot)\A_xDot'*y_xDot # the identity matrix is used to scale the coefficients - mpcCoeff.c_Vy = (A_yDot'*A_yDot)\A_yDot'*y_yDot + mpcCoeff.c_Psi = (A_psi'*A_psi + lambda1*eye(size(A_psi'*A_psi)[1],size(A_psi'*A_psi)[2]) )\A_psi'*y_psi + mpcCoeff.c_Vx = (A_xDot'*A_xDot + lambda2*eye(size(A_xDot'*A_xDot)[1],size(A_xDot'*A_xDot)[2]) )\A_xDot'*y_xDot # the identity matrix is used to scale the coefficients + mpcCoeff.c_Vy = (A_yDot'*A_yDot + lambda3*eye(size(A_yDot'*A_yDot)[1],size(A_yDot'*A_yDot)[2]) )\A_yDot'*y_yDot # if det(A_psi'*A_psi) != 0 # else diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index f0d4acd4..08b17d05 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -47,7 +47,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track posInfo::PosInfo,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,lapStatus::LapStatus,buffersize::Int64, obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) selectedStates.simulator = true # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use - obstacle.obstacle_tuning = true # set this to TRUE if the tuning for obstacle avoidance is needed, FALSE if not needed + obstacle.obstacle_tuning = false # set this to TRUE if the tuning for obstacle avoidance is needed, FALSE if not needed if selectedStates.simulator == false # if the BARC is in use @@ -105,23 +105,27 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull - selectedStates.shift = 5 + selectedStates.Nl_sID = 3 + selectedStates.lambda1 = 1 + selectedStates.lambda2 = 1 + selectedStates.lambda3 = 1 + selectedStates.shift = 8 Nl = selectedStates.Nl selectedStates.selStates = zeros(Nl*selectedStates.Np,6) selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 13 + mpcParams.N = 14 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states mpcParams.QderivU = 1.0*[1,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 1 # scaling of Q-function + mpcParams.Q_term_cost = 3 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay - mpcParams.Q_lane = 3 # weight on the soft constraint for the lane + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity mpcParams.Q_slack = 1*[20.0,1.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories @@ -209,8 +213,8 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) - obstacle.lap_deactivate = 6 # number of lap in which to stop considering obstacles - obstacle.lap_active = 4 # number of the first lap in which the obstacles are used + obstacle.lap_deactivate = 600 # number of lap in which to stop considering obstacles + obstacle.lap_active = 400 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles obstacle.s_obs_init = [15] # initial s coordinate of each obstacle diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index 55f5b582..2b58990d 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -62,7 +62,11 @@ type SelectedStates # Values needed for the convex hull formulat version::Bool # false if you want to use convex hull simulator::Bool # true to use tuning made for simulator, false to use tuning made for BARC shift::Int64 - SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false, simulator=true,shift=3) = new(selStates,statesCost,Np,Nl,version,simulator,shift) + Nl_sID::Int64 + lambda1::Int64 + lambda2::Int64 + lambda3::Int64 + SelectedStates(selStates=Float64[],statesCost=Float64[],Np=6,Nl=2,version=false, simulator=true,shift=3,Nl_sID=3,lambda1=1,lambda2=1,lambda3=1) = new(selStates,statesCost,Np,Nl,version,simulator,shift,Nl_sID,lambda1,lambda2,lambda3) end type MpcParams # parameters for MPC solver From 7771e72ca81009a35bc01e49f96c6779fe3c25a0 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Fri, 16 Feb 2018 16:30:08 -0800 Subject: [PATCH 159/183] best tuning --- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 08b17d05..60baf9b7 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -59,20 +59,20 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track selectedStates.statesCost = zeros(Nl*selectedStates.Np) selectedStates.version = false - mpcParams.N = 14 + mpcParams.N = 12 mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) mpcParams.vPathFollowing = 1 # reference speed for first lap of path following mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states - mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs - mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.QderivU = 1.0*[5.0,0.1] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 5 # scaling of Q-function mpcParams.delay_df = 3 # steering delay mpcParams.delay_a = 1 # acceleration delay mpcParams.Q_lane = 1 # weight on the soft constraint for the lane mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity - mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s - mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories + mpcParams.Q_slack = 1*[5*20.0,0.5*20.0,1*10.0,30.0,0.1*80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories if obstacle.obstacle_tuning == true From 84362eb526900bf5c31cd235eab8883b446f48f7 Mon Sep 17 00:00:00 2001 From: Ugo Date: Sat, 17 Feb 2018 13:07:32 -0800 Subject: [PATCH 160/183] Good Tuning --- .../barc/src/barc_lib/LMPC/coeffConstraintCost.jl | 14 +++++++------- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 8 ++++++-- .../src/state_estimation_SensorKinematicModel.py | 3 ++- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl index ef3897a8..73270f5d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/coeffConstraintCost.jl @@ -252,7 +252,7 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo sz1 = size(sysID_idx,1) sz2 = size(sysID_idx_c,1) - #sz2 = 0 + sz2 = 0 sz = sz1 + sz2 # psiDot @@ -261,8 +261,8 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo for i=0:freq_ratio-1 y_psi[(1:sz1)+i*sz1] = diff(oldpsiDot_sID[sysID_idx_diff+i]) A_psi[(1:sz1)+i*sz1,:] = [oldpsiDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] oldyDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] olddF_sID[sysID_idx+i]] - y_psi[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) - A_psi[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2,cL]] + #y_psi[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL]) + #A_psi[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2,cL]] y_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2] = diff(oldpsiDot_sID[sysID_idx_diff2+i]) A_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2,:] = [oldpsiDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] oldyDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] olddF_sID[sysID_idx2+i]] @@ -277,8 +277,8 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo for i=0:freq_ratio-1 y_xDot[(1:sz1)+i*sz1] = diff(oldxDot_sID[sysID_idx_diff+i]) A_xDot[(1:sz1)+i*sz1,:] = [oldyDot_sID[sysID_idx+i].*oldpsiDot_sID[sysID_idx+i] oldxDot_sID[sysID_idx+i] oldacc_sID[sysID_idx+i]] - y_xDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) - A_xDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] + #y_xDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL]) + #A_xDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,7,cL]] y_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldxDot_sID[sysID_idx_diff2+i]) A_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot_sID[sysID_idx2+i].*oldpsiDot_sID[sysID_idx2+i] oldxDot_sID[sysID_idx2+i] oldacc_sID[sysID_idx2+i]] @@ -293,8 +293,8 @@ function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo for i=0:freq_ratio-1 y_yDot[(1:sz1)+i*sz1] = diff(oldyDot_sID[sysID_idx_diff+i]) A_yDot[(1:sz1)+i*sz1,:] = [oldyDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] oldpsiDot_sID[sysID_idx+i].*oldxDot_sID[sysID_idx+i] oldpsiDot_sID[sysID_idx+i]./oldxDot_sID[sysID_idx+i] olddF_sID[sysID_idx+i-delay_df*freq_ratio]] - y_yDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) - A_yDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2]] + #y_yDot[(1:sz2)+i*sz2+freq_ratio*sz1] = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL]) + #A_yDot[(1:sz2)+i*sz2+freq_ratio*sz1,:] = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2]] y_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)] = diff(oldyDot_sID[sysID_idx_diff2+i]) A_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] oldpsiDot_sID[sysID_idx2+i].*oldxDot_sID[sysID_idx2+i] oldpsiDot_sID[sysID_idx2+i]./oldxDot_sID[sysID_idx2+i] olddF_sID[sysID_idx2+i-delay_df*5]] diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 237c7fcf..33e96691 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -48,11 +48,15 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track obstacle::Obstacle,selectedStates::SelectedStates,oldSS::SafeSetData) selectedStates.simulator = false # set this to TRUE if SIMULATOR is in use, set this to FALSE if BARC is in use - obstacle.obstacle_tuning = true # set this to TRUE if the tuning for obstacle avoidance is needed, FALSE if not needed + obstacle.obstacle_tuning = false # set this to TRUE if the tuning for obstacle avoidance is needed, FALSE if not needed if selectedStates.simulator == false # if the BARC is in use - selectedStates.Np = 15 # Number of points to take from each previous trajectory to build the convex hull + selectedStates.Nl_sID = 3 + selectedStates.lambda1 = 0 + selectedStates.lambda2 = 0 + selectedStates.lambda3 = 0 + selectedStates.Np = 20 # Number of points to take from each previous trajectory to build the convex hull selectedStates.Nl = 2 # Number of previous laps to include in the convex hull selectedStates.shift = 8 Nl = selectedStates.Nl diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py index dc2d9867..7090d85d 100755 --- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py @@ -224,7 +224,8 @@ def state_estimation(): Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) - #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) + R = diag([4*5.0,4*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 4*5.0,4*5.0,10.0,1.0, 10.0,10.0]) + R = diag([1*5.0,1*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 1*5.0,1*5.0,10.0,1.0, 10.0,10.0]) # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v # Set up track parameters From a98e9c11d8d0d7ee0419e1451a5fa768b268089d Mon Sep 17 00:00:00 2001 From: Ugo Date: Sat, 17 Feb 2018 13:19:29 -0800 Subject: [PATCH 161/183] Changes in eval function --- eval_sim/eval_data.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 0804c779..99494997 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -819,6 +819,7 @@ function plot_v_over_xy(code::AbstractString,lap::Int64) cb = colorbar() cb[:set_label]("Velocity [m/s]") println("Average v_x = ",mean(pos_info.z[idx,8])," m/s") + println("Max v_x = ",findmax(pos_info.z[idx,8])[1]," m/s") end From 9fe78457d13eaead5e3c631140c4aa62beea61f1 Mon Sep 17 00:00:00 2001 From: Jon Gonzales Date: Thu, 1 Mar 2018 02:22:39 -0800 Subject: [PATCH 162/183] remote commit for francesco --- eval_sim/eval_data.jl | 103 +++++++++++++++--- .../src/barc/src/barc_lib/LMPC/functions.jl | 8 +- workspace/src/barc/src/debug_localization.py | 2 + 3 files changed, 94 insertions(+), 19 deletions(-) diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 0804c779..496caad0 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -96,13 +96,17 @@ function eval_sim(code::AbstractString) grid() end -function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obstacle::Bool) +function eval_convhull(code::AbstractString,code2::AbstractString,laps::Array{Int64},switch::Bool,obstacle::Bool) log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" - + log_path_LMPC2 = "$(homedir())/simulations/output-LMPC-$(code2).jld" + Data = load(log_path_LMPC) + Data2 = load(log_path_LMPC2) oldSS_xy = Data["oldSS_xy"] + oldSS_xy2 = Data2["oldSS_xy"] + oldSS = Data["oldSS"] selectedStates = Data["selectedStates"] selStates = Data["selStates"] @@ -132,7 +136,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst flag = zeros(2) - track = create_track(0.4) + track = create_track(0.45) pred_horizon = size(pred_sol)[1] - 1 @@ -182,8 +186,63 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst title("Slack costs") grid("on") + pred_sol_xy, xyPathAngle = xyObstacle(oldSS,obs_log,1,11,track) + ellfig = figure(20) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,11],oldSS_xy[:,2,11],oldSS_xy2[:,1,22],oldSS_xy2[:,2,22]) + legend(["lap 13","lap 26"]) + xlabel("x [m]") + ylabel("y [m]") + grid("on") + + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + ax[:add_artist](ell1) + title("X-Y view of laps 13 and 26") + ellfig = figure(21) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,5],oldSS_xy[:,2,5],oldSS_xy[:,1,26],oldSS_xy[:,2,26]) + legend(["lap 5","lap 26"]) + xlabel("x [m]") + ylabel("y [m]") + grid("on") + + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + # ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + #ax[:add_artist](ell1) + title("X-Y view of laps 5 and 26") + + + ellfig = figure(22) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy2[:,1,10],oldSS_xy2[:,2,10],oldSS_xy2[:,1,22],oldSS_xy2[:,2,22]) + legend(["lap 14","lap 26"]) + xlabel("x [m]") + ylabel("y [m]") + grid("on") + + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) + ax[:add_artist](ell1) + title("X-Y view of laps 14 and 26") + for i = laps @@ -238,7 +297,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 90) + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 0) ax[:add_artist](ell1) end @@ -383,7 +442,7 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst subplot(222) plot(t,cvy1,t,cvy2,t,cvy3,t,cvy4) - legend(["cvx1","cvx2","cvx3","cvy4"]) + legend(["cvy1","cvy2","cvy3","cvy4"]) title("C_Vy in lap $i") grid("on") @@ -409,8 +468,8 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst - for j = 2:2000 - + # for j = 2:2000 + for j = 54:60 solution_status = status[final_counter[i-1]+j] vx_pred = pred_sol[:,1,j,i] @@ -474,7 +533,17 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst clf() # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + #plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i]) + + # plot([oldSS_xy[63:75,1,i-1],oldSS_xy[63:75,1,i-2]],[oldSS_xy[63:75,2,i-1],oldSS_xy[63:75,2,i-2]],"g")#,oldSS_xy[:,1,i-2],oldSS_xy[:,2,i-2]) + plot(oldSS_xy[59:73,1,i-1],oldSS_xy[59:73,2,i-1],"g") + legend(["SS^13"]) + plot(oldSS_xy[63:77,1,i-2],oldSS_xy[63:77,2,i-2],"g") plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + plot(xy_predictions[1,:]',xy_predictions[2,:]',"k*") + + xlabel("x[m]") + ylabel("y[m]") #for i=1:4:size(x_est,1) # for index=1:length(oldSS_xy[:,1,i]) @@ -488,20 +557,22 @@ function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obst ax = ellfig[:add_subplot](1,1,1) ax[:set_aspect]("equal") - plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + #plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") - plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + #plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") angle_ell = atan2(pred_sol_xy[2,j+1]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j+1]-(pred_sol_xy[1,j-1])) angle_deg = (angle_ell*180)/pi - ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.4, 0.2, angle = angle_deg) + ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.4, 0.2, angle = 0) + rec1 = patch.Rectangle([xy_predictions[1,1]'-0.2,xy_predictions[2,1]'-0.1],0.4,0.2,angle=-2,fill=false) ax[:add_artist](ell1) + ax[:add_artist](rec1) - plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") - grid("on") + #plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + #grid("on") # title("Predicted solution in lap $i, iteration $j") - title("Position at iteration $j") + title("Prediction in XY, lap 13") # end end @@ -785,7 +856,9 @@ function plot_v_ey_over_s(code::AbstractString,laps::Array{Int64}) lap_times[i] = t_end-t_start end figure(3) - plot(1:size(lap_times,1),lap_times,"-o") + #plot(1:size(lap_times,1)-2,lap_times[1:size(lap_times,1)-2],"-o") + plot(1:4,lap_times[1:4],"go",5:13,lap_times[5:13],"bo",14:size(lap_times,1),lap_times[14:size(lap_times,1)],"ro") + legend(["Path following","LMPC with Obstacle","LMPC without Obstacle"]) grid("on") xlabel("Lap number") ylabel("Lap time [s]") @@ -812,7 +885,7 @@ function plot_v_over_xy(code::AbstractString,lap::Int64) figure() plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") - scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=0.5,vmax=3.0) + scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=1.5,vmax=2.8) grid(1) title("x-y-view") axis("equal") diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index 60baf9b7..2e0d2c2f 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -213,12 +213,12 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) - obstacle.lap_deactivate = 600 # number of lap in which to stop considering obstacles - obstacle.lap_active = 400 # number of the first lap in which the obstacles are used + obstacle.lap_deactivate = 6 # number of lap in which to stop considering obstacles + obstacle.lap_active = 4 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles - obstacle.s_obs_init = [15] # initial s coordinate of each obstacle - obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle + obstacle.s_obs_init = [12] # initial s coordinate of each obstacle + obstacle.ey_obs_init = [-0.35] # initial ey coordinate of each obstacle obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.4#0.5 obstacle.r_ey = 0.2#0.2 diff --git a/workspace/src/barc/src/debug_localization.py b/workspace/src/barc/src/debug_localization.py index 148b14cb..f91f6545 100644 --- a/workspace/src/barc/src/debug_localization.py +++ b/workspace/src/barc/src/debug_localization.py @@ -10,6 +10,8 @@ plt.plot(l.nodes[0,:],l.nodes[1,:],"r-o") plt.grid('on') plt.axis('equal') +plt.xlabel('x [m]') +plt.ylabel('y [m]') plt.show() From 3279faca8af8bbf5c97a04cf3a0426e0e5584121 Mon Sep 17 00:00:00 2001 From: lukasbrunke Date: Fri, 16 Mar 2018 08:59:36 -0700 Subject: [PATCH 163/183] Setting up communication for two machines for testing --- env_loader_yoga.sh | 10 ++++ .../src/barc/launch/barc_multi_sim.launch | 48 +++++++++++++++++++ workspace/src/barc/msg/prediction.msg | 6 +++ workspace/src/barc/src/test_connection.py | 35 ++++++++++++++ 4 files changed, 99 insertions(+) create mode 100755 env_loader_yoga.sh create mode 100755 workspace/src/barc/launch/barc_multi_sim.launch create mode 100644 workspace/src/barc/msg/prediction.msg create mode 100755 workspace/src/barc/src/test_connection.py diff --git a/env_loader_yoga.sh b/env_loader_yoga.sh new file mode 100755 index 00000000..6f80f51a --- /dev/null +++ b/env_loader_yoga.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +source ~/barc/scripts/startup.sh +source /opt/ros/kinetic/setup.bash +source ~/barc/workspace/devel/setup.bash + +export ROS_IP=192.168.10.74 +export ROS_MASTER_URI=http://192.168.10.147:11311 + +exec "$@" diff --git a/workspace/src/barc/launch/barc_multi_sim.launch b/workspace/src/barc/launch/barc_multi_sim.launch new file mode 100755 index 00000000..9e56b9c9 --- /dev/null +++ b/workspace/src/barc/launch/barc_multi_sim.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/msg/prediction.msg b/workspace/src/barc/msg/prediction.msg new file mode 100644 index 00000000..7d7a686a --- /dev/null +++ b/workspace/src/barc/msg/prediction.msg @@ -0,0 +1,6 @@ +# This is a message to communicate the agent's prediction +Header header +float64[] s +float64[] ey +float64[] epsi +float64[] v \ No newline at end of file diff --git a/workspace/src/barc/src/test_connection.py b/workspace/src/barc/src/test_connection.py new file mode 100755 index 00000000..fea546d1 --- /dev/null +++ b/workspace/src/barc/src/test_connection.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Author: Lukas Brunke +# Email: lukas.brunke@tuhh.de +# +# --------------------------------------------------------------------------- + +import time +import rospy +from barc.msg import prediction + + +def callback(test): + rospy.loginfo(test) + + +def receiving_prediction(): + rospy.init_node("connection_test", anonymous=True) + rospy.Subscriber("prediction", prediction, callback) + + while not rospy.is_shutdown(): + rospy.spin() + + +if __name__ == '__main__': + try: + receiving_prediction() + except rospy.ROSInterruptException: + pass From 0cf19e77bf3d2479087ab593486b2bcee6a0dc8b Mon Sep 17 00:00:00 2001 From: lukasbrunke Date: Fri, 16 Mar 2018 09:17:16 -0700 Subject: [PATCH 164/183] syncing connection test files --- workspace/src/barc/launch/barc_multi_sim.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/workspace/src/barc/launch/barc_multi_sim.launch b/workspace/src/barc/launch/barc_multi_sim.launch index 9e56b9c9..a3cbfb68 100755 --- a/workspace/src/barc/launch/barc_multi_sim.launch +++ b/workspace/src/barc/launch/barc_multi_sim.launch @@ -8,7 +8,7 @@ - + From 9a2848f6f012d1d1d05e8b120b057b98cd591009 Mon Sep 17 00:00:00 2001 From: lukasbrunke Date: Mon, 19 Mar 2018 16:18:12 -0700 Subject: [PATCH 165/183] Fixed the crashing of the visualization and added a test for pubslishing obstacle data --- env_loader_pc.sh | 6 +-- scripts/startup.sh | 14 +++--- workspace/src/barc/CMakeLists.txt | 1 + workspace/src/barc/src/LMPC_node.jl | 26 ++++++++-- .../src/barc/src/barc_lib/LMPC/functions.jl | 8 ++-- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 7 ++- workspace/src/barc/src/test_obstacle.jl | 48 +++++++++++++++++++ workspace/src/barc/src/view_car_trajectory.py | 3 +- 8 files changed, 93 insertions(+), 20 deletions(-) create mode 100644 workspace/src/barc/src/test_obstacle.jl diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 9b8ac9dd..ddf2680d 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -1,7 +1,7 @@ #!/bin/bash source /opt/ros/kinetic/setup.bash -source /home/ugo/GitHub/barc/workspace/devel/setup.bash +source /home/mpcubuntu/barc/workspace/devel/setup.bash #export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=10.0.0.72 -export ROS_MASTER_URI=http://10.0.0.72:11311 +export ROS_IP=192.168.10.147 +export ROS_MASTER_URI=http://192.168.10.147:11311 exec "$@" diff --git a/scripts/startup.sh b/scripts/startup.sh index 16d1c109..4331836b 100755 --- a/scripts/startup.sh +++ b/scripts/startup.sh @@ -2,19 +2,19 @@ # local start up script when opening bash session # set environment variable for python -export WORKON_HOME=$HOME/.virtualenvs -export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python -export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv -export VIRTUALENVWRAPPER_VIRTUALENV_ARGS='--no-site-packages' -source /usr/local/bin/virtualenvwrapper.sh -workon barc +# export WORKON_HOME=$HOME/.virtualenvs +# export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python +# export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv +# export VIRTUALENVWRAPPER_VIRTUALENV_ARGS='--no-site-packages' +# source /usr/local/bin/virtualenvwrapper.sh +# workon barc # set environment variables for ROS source $HOME/barc/workspace/devel/setup.bash export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ # set environment variables for Amazon Web Server -source $HOME/team_name.sh +# source $HOME/team_name.sh # define commands # * nanorst - resets the arduino nano from the command line (assuming the device is connected and on port /dev/ttyUSB0 diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index b864a6d3..3f405e3f 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -50,6 +50,7 @@ add_message_files( pos_info.msg ECU.msg Vel_est.msg + prediction.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 9494ca30..926ba71e 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -1,7 +1,7 @@ #!/usr/bin/env julia using RobotOS -@rosimport barc.msg: ECU, pos_info +@rosimport barc.msg: ECU, pos_info, prediction @rosimport geometry_msgs.msg: Vector3 rostypegen() using barc.msg @@ -67,6 +67,10 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po end end +function obstacle_callback(msg::prediction) + return prediction[:, [1, 2, 4]] +end + # This is the main function, it is called when the node is started. function main() println("Starting LMPC node.") @@ -110,6 +114,8 @@ function main() coeffX = zeros(9) # buffer for coeffX (only logging) coeffY = zeros(9) # buffer for coeffY (only logging) cmd = ECU() # command type + agents_predictions = prediction() + obstacle_predictions = prediction() coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables @@ -155,10 +161,13 @@ function main() init_node("mpc_traj") loop_rate = Rate(1/modelParams.dt) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + pub_prediction = Publisher("prediction", prediction, queue_size=1)::RobotOS.Publisher{barc.msg.prediction} # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,trackCoeff,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! + sub_obstacle = Subscriber("obstacle", obstacle_predictions, obstacle_callback, queue_size=1)::RobotOS.Subscriber{barc.msg.prediction} + run_id = get_param("run_id") println("Finished initialization.") @@ -412,6 +421,8 @@ function main() ## if obstacles are on the track, find the nearest one if obstacle.obstacle_active == true + # take the first input from the obstacle prediction + obs_curr[lapStatus.currentIt, :, 1] = sub_obstacle[1, :] obs_temp = obs_curr[lapStatus.currentIt,:,:] @@ -428,6 +439,7 @@ function main() dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) # find the closest obstacle using the equation of the ellipse. Closest in terms of s and e_y!! obs_near = obs_temp[1,:,index] + obs_near = sub_obstacle # println("current s= ",posInfo.s) # println("closest obstacle= ",obs_near[1,1,1]) @@ -479,9 +491,17 @@ function main() log_t_solv[k+1] = toq() #println("time= ",log_t_solv[k+1]) + agents_predictions.header.stamp = get_rostime() + agents_predictions.s = mpcSol.z[:, 1] + agents_predictions.ey = mpcSol.z[:, 2] + agents_predictions.epsi = mpcSol.z[:, 3] + agents_predictions.v = mpcSol.z[:, 4] + publish(pub_prediction, agents_predictions) + + #= if obstacle.obstacle_active == true - obs_curr[lapStatus.currentIt+1,:,:] = obstaclePosition(obs_curr[i,:,:],modelParams,obstacle,posInfo) - end + # obs_curr[lapStatus.currentIt+1,:,:] = obstaclePosition(obs_curr[i,:,:],modelParams,obstacle,posInfo) end + =# # Send command immediately, only if it is optimal! #if mpcSol.solverStatus == :Optimal diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index db814cb3..d435a76d 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -215,14 +215,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) -<<<<<<< HEAD - obstacle.lap_deactivate = 6 # number of lap in which to stop considering obstacles + obstacle.lap_deactivate = 8 # number of lap in which to stop considering obstacles obstacle.lap_active = 4 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles obstacle.s_obs_init = [12] # initial s coordinate of each obstacle obstacle.ey_obs_init = [-0.35] # initial ey coordinate of each obstacle -======= + +#= obstacle.lap_deactivate = 600 # number of lap in which to stop considering obstacles obstacle.lap_active = 400 # number of the first lap in which the obstacles are used @@ -230,7 +230,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track obstacle.n_obs = 1 # number of obstacles obstacle.s_obs_init = [18] # initial s coordinate of each obstacle obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle ->>>>>>> ac91d9d1865a2a4ebc4e95581c82471ef3176693 +=# obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.4#0.5 obstacle.r_ey = 0.2#0.2 diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl index 578227bc..1b2661f1 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -185,7 +185,8 @@ function solveMpcProblem_obstacle(m::MpcModel_obstacle,mpcSol::MpcSol,mpcCoeff:: Q_obs = mpcParams.Q_obs::Array{Float64,1} # println("Q_obs= ",Q_obs) - + + #= obs = zeros(mpcParams.N+1,3) obs[1,:] = obs_now @@ -196,8 +197,10 @@ function solveMpcProblem_obstacle(m::MpcModel_obstacle,mpcSol::MpcSol,mpcCoeff:: obs[i+1,2] = obs[i,2] obs[i+1,3] = obs[i,3] end + =# - # println("obstacle= ",obs) + obs = obs_now + println("obstacle= ",obs) # Update current initial condition, curvature and System ID coefficients setvalue(m.z0,zCurr) diff --git a/workspace/src/barc/src/test_obstacle.jl b/workspace/src/barc/src/test_obstacle.jl new file mode 100644 index 00000000..71587798 --- /dev/null +++ b/workspace/src/barc/src/test_obstacle.jl @@ -0,0 +1,48 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, prediction +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/coeffConstraintCost.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") +include("barc_lib/obstaclePosition.jl") + +# horizon +const OBSTACLE_N = 16 + +function obstacle() + obstacle_predictions = prediction() + + init_node("obstacle") + loop_rate = Rate(10.0) + + obstacle = Publisher("obs_prediction", prediction, queue_size=1)::RobotOS.Publisher{barc.msg.prediction} + + while !is_shutdown() + obstacle_predictions.header.stamp = get_rostime() + obstacle_predictions.s = 12 * ones(OBSTACLE_N + 1) + obstacle_predictions.ey = - 0.35 * ones(OBSTACLE_N + 1) + obstacle_predictions.epsi = 0.0 * ones(OBSTACLE_N + 1) + obstacle_predictions.v = 0.0 * ones(OBSTACLE_N + 1) + + publish(obstacle, obstacle_predictions) + + rossleep(loop_rate) + end +end + +if !isinteractive() + obstacle() +end \ No newline at end of file diff --git a/workspace/src/barc/src/view_car_trajectory.py b/workspace/src/barc/src/view_car_trajectory.py index a747e363..e9575a7d 100755 --- a/workspace/src/barc/src/view_car_trajectory.py +++ b/workspace/src/barc/src/view_car_trajectory.py @@ -140,7 +140,8 @@ def view_trajectory(): if (v_vals): t_vals_zeroed = [t - t_vals[0] for t in t_vals] - ax2.plot(t_vals_zeroed, v_vals, 'm-') + last_value = min(len(t_vals_zeroed), len(v_vals)) + ax2.plot(t_vals_zeroed[:last_value], v_vals[:last_value], 'm-') ax2.set_ylim([min(0, min(v_vals)), max(vmax_ref, max(v_vals))]) From 58220cc2f9b635475e32cc174376b7ce85d1fe75 Mon Sep 17 00:00:00 2001 From: lukasbrunke Date: Mon, 19 Mar 2018 16:25:41 -0700 Subject: [PATCH 166/183] preparation for publishing obstacle states --- env_loader_pc.sh | 9 ++++++--- workspace/src/barc/CMakeLists.txt | 1 + workspace/src/barc/launch/barc_multi_sim.launch | 4 ++-- workspace/src/barc/src/LMPC_node.jl | 11 ++++++++++- workspace/src/barc/src/barc_lib/LMPC/functions.jl | 6 +++--- workspace/src/barc/src/view_car_trajectory.py | 3 ++- 6 files changed, 24 insertions(+), 10 deletions(-) diff --git a/env_loader_pc.sh b/env_loader_pc.sh index 9b8ac9dd..5656d203 100755 --- a/env_loader_pc.sh +++ b/env_loader_pc.sh @@ -1,7 +1,10 @@ #!/bin/bash + source /opt/ros/kinetic/setup.bash -source /home/ugo/GitHub/barc/workspace/devel/setup.bash +source /home/mpcubuntu/barc/workspace/devel/setup.bash #export PATH=$PATH:/home/ugo/julia/bin/julia -export ROS_IP=10.0.0.72 -export ROS_MASTER_URI=http://10.0.0.72:11311 + +export ROS_IP=192.168.10.147 +export ROS_MASTER_URI=http://192.168.10.147:11311 + exec "$@" diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index b864a6d3..3f405e3f 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -50,6 +50,7 @@ add_message_files( pos_info.msg ECU.msg Vel_est.msg + prediction.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_multi_sim.launch b/workspace/src/barc/launch/barc_multi_sim.launch index a3cbfb68..0cca6647 100755 --- a/workspace/src/barc/launch/barc_multi_sim.launch +++ b/workspace/src/barc/launch/barc_multi_sim.launch @@ -42,7 +42,7 @@ - - + + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 9494ca30..d4b223bb 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -1,7 +1,7 @@ #!/usr/bin/env julia using RobotOS -@rosimport barc.msg: ECU, pos_info +@rosimport barc.msg: ECU, pos_info, prediction @rosimport geometry_msgs.msg: Vector3 rostypegen() using barc.msg @@ -110,6 +110,7 @@ function main() coeffX = zeros(9) # buffer for coeffX (only logging) coeffY = zeros(9) # buffer for coeffY (only logging) cmd = ECU() # command type + agents_predictions = prediction() coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables @@ -155,6 +156,7 @@ function main() init_node("mpc_traj") loop_rate = Rate(1/modelParams.dt) pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + pub_prediction = Publisher("prediction", prediction, queue_size=1)::RobotOS.Publisher{barc.msg.prediction} # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,trackCoeff,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! @@ -479,6 +481,13 @@ function main() log_t_solv[k+1] = toq() #println("time= ",log_t_solv[k+1]) + agents_predictions.header.stamp = get_rostime() + agents_predictions.s = mpcSol.z[:, 1] + agents_predictions.ey = mpcSol.z[:, 2] + agents_predictions.epsi = mpcSol.z[:, 3] + agents_predictions.v = mpcSol.z[:, 4] + publish(pub_prediction, agents_predictions) + if obstacle.obstacle_active == true obs_curr[lapStatus.currentIt+1,:,:] = obstaclePosition(obs_curr[i,:,:],modelParams,obstacle,posInfo) end diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl index db814cb3..d0e7b6a7 100644 --- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -215,14 +215,14 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track lapStatus.currentIt = 0 # current iteration in lap obstacle.obstacle_active = false # true if we have to consider the obstacles in the optimization problem (NEVER set true here, the LMPC_node.jl script will set this value to true as soon as the current lap is equal to obstacle.lap_active) -<<<<<<< HEAD obstacle.lap_deactivate = 6 # number of lap in which to stop considering obstacles obstacle.lap_active = 4 # number of the first lap in which the obstacles are used obstacle.obs_detect = 10 # maximum distance at which we can detect obstacles (in terms of s!!) obstacle.n_obs = 1 # number of obstacles obstacle.s_obs_init = [12] # initial s coordinate of each obstacle obstacle.ey_obs_init = [-0.35] # initial ey coordinate of each obstacle -======= + +#= obstacle.lap_deactivate = 600 # number of lap in which to stop considering obstacles obstacle.lap_active = 400 # number of the first lap in which the obstacles are used @@ -230,7 +230,7 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track obstacle.n_obs = 1 # number of obstacles obstacle.s_obs_init = [18] # initial s coordinate of each obstacle obstacle.ey_obs_init = [-0.3] # initial ey coordinate of each obstacle ->>>>>>> ac91d9d1865a2a4ebc4e95581c82471ef3176693 +=# obstacle.v_obs_init = [0] # initial velocity of each obstacles obstacle.r_s = 0.4#0.5 obstacle.r_ey = 0.2#0.2 diff --git a/workspace/src/barc/src/view_car_trajectory.py b/workspace/src/barc/src/view_car_trajectory.py index a747e363..e9575a7d 100755 --- a/workspace/src/barc/src/view_car_trajectory.py +++ b/workspace/src/barc/src/view_car_trajectory.py @@ -140,7 +140,8 @@ def view_trajectory(): if (v_vals): t_vals_zeroed = [t - t_vals[0] for t in t_vals] - ax2.plot(t_vals_zeroed, v_vals, 'm-') + last_value = min(len(t_vals_zeroed), len(v_vals)) + ax2.plot(t_vals_zeroed[:last_value], v_vals[:last_value], 'm-') ax2.set_ylim([min(0, min(v_vals)), max(vmax_ref, max(v_vals))]) From 3a312fa66a905864b966a52ae30581fd044f8bff Mon Sep 17 00:00:00 2001 From: lukasbrunke Date: Tue, 20 Mar 2018 14:28:51 -0700 Subject: [PATCH 167/183] Added node for sending obstacle information --- .../src/barc/launch/barc_multi_sim.launch | 2 +- workspace/src/barc/src/LMPC_node.jl | 24 ++++++++++++------- workspace/src/barc/src/barc_lib/classes.jl | 10 ++++++-- workspace/src/barc/src/test_obstacle.jl | 5 ++-- 4 files changed, 27 insertions(+), 14 deletions(-) diff --git a/workspace/src/barc/launch/barc_multi_sim.launch b/workspace/src/barc/launch/barc_multi_sim.launch index 0cca6647..cbc097ef 100755 --- a/workspace/src/barc/launch/barc_multi_sim.launch +++ b/workspace/src/barc/launch/barc_multi_sim.launch @@ -43,6 +43,6 @@ - + diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl index 718ac0fe..b7702cc3 100755 --- a/workspace/src/barc/src/LMPC_node.jl +++ b/workspace/src/barc/src/LMPC_node.jl @@ -67,8 +67,11 @@ function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,po end end -function obstacle_callback(msg::prediction) - return prediction[:, [1, 2, 4]] +function obstacle_callback(msg::prediction, obstacle::Obstacle, dummy::Int64) # update current position and track data +#function obstacle_callback(msg::prediction, obstacle::Obstacle) + obstacle.prediction[:, 1] = msg.s + obstacle.prediction[:, 2] = msg.ey + obstacle.prediction[:, 3] = msg.v end # This is the main function, it is called when the node is started. @@ -115,10 +118,8 @@ function main() coeffY = zeros(9) # buffer for coeffY (only logging) cmd = ECU() # command type agents_predictions = prediction() -<<<<<<< HEAD -======= - obstacle_predictions = prediction() ->>>>>>> b0c95e078574a78abb30a26f3cda480f50676e4f + # obstacle_predictions = prediction() + obstacle.prediction = zeros(mpcParams.N + 1, 3) coeffCurvature_update = zeros(trackCoeff.nPolyCurvature+1) # Logging variables @@ -169,7 +170,11 @@ function main() s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,trackCoeff,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! - sub_obstacle = Subscriber("obstacle", obstacle_predictions, obstacle_callback, queue_size=1)::RobotOS.Subscriber{barc.msg.prediction} + #sub_obstacle = Subscriber("obs_prediction", prediction, obstacle_callback, callback_args=(obstacle), + # queue_size=1)::RobotOS.Subscriber{barc.msg.prediction} + dummy = 0 + s2 = Subscriber("obs_prediction", prediction, obstacle_callback, (obstacle, dummy), queue_size=1)::RobotOS.Subscriber{barc.msg.prediction} + run_id = get_param("run_id") println("Finished initialization.") @@ -422,10 +427,11 @@ function main() end ## if obstacles are on the track, find the nearest one + println(obstacle.prediction[:, 1]) if obstacle.obstacle_active == true # take the first input from the obstacle prediction - obs_curr[lapStatus.currentIt, :, 1] = sub_obstacle[1, :] + obs_curr[lapStatus.currentIt, :, 1] = obstacle.prediction[1, :] obs_temp = obs_curr[lapStatus.currentIt,:,:] @@ -442,7 +448,7 @@ function main() dist,index=findmin(sqrt((obs_temp[1,1,:]-zCurr[lapStatus.currentIt,6]).^2 + (obs_temp[1,2,:]-zCurr[lapStatus.currentIt,5]).^2)) # find the closest obstacle using the equation of the ellipse. Closest in terms of s and e_y!! obs_near = obs_temp[1,:,index] - obs_near = sub_obstacle + obs_near = obstacle.prediction # println("current s= ",posInfo.s) # println("closest obstacle= ",obs_near[1,1,1]) diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl index 2b58990d..e90a6db4 100644 --- a/workspace/src/barc/src/barc_lib/classes.jl +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -146,6 +146,12 @@ type Obstacle r_ey::Float64 # radius on the ey coordinate of the ellipse describing the obstacle inv_step::Int64 # number of step of invariance required for the safe set obstacle_tuning::Bool # true if we are using the tuning made for obstacle avoidance - - Obstacle(obstacle_active=false,lap_deactivate=12,lap_active=10,obs_detect=1.0,n_obs=1,s_obs_init=Float64[],ey_obs_init=Float64[],v_obs_init=Float64[],r_s=0.5,r_ey=0.3,inv_step=1,obstacle_tuning=false) = new(obstacle_active,lap_deactivate,lap_active,obs_detect,n_obs,s_obs_init,ey_obs_init,v_obs_init,r_s,r_ey,inv_step,obstacle_tuning) + prediction::Array{Float64} + + Obstacle(obstacle_active=false, lap_deactivate=12, lap_active=10, obs_detect=1.0, + n_obs=1, s_obs_init=Float64[], ey_obs_init=Float64[], v_obs_init=Float64[], + r_s=0.5, r_ey=0.3, inv_step=1, obstacle_tuning=false, + prediction=Float64[]) = new(obstacle_active, + lap_deactivate, lap_active, obs_detect, n_obs, s_obs_init, ey_obs_init, + v_obs_init, r_s, r_ey, inv_step, obstacle_tuning, prediction) end \ No newline at end of file diff --git a/workspace/src/barc/src/test_obstacle.jl b/workspace/src/barc/src/test_obstacle.jl index 71587798..233fe4d6 100644 --- a/workspace/src/barc/src/test_obstacle.jl +++ b/workspace/src/barc/src/test_obstacle.jl @@ -20,7 +20,7 @@ include("barc_lib/simModel.jl") include("barc_lib/obstaclePosition.jl") # horizon -const OBSTACLE_N = 16 +const OBSTACLE_N = 12 function obstacle() obstacle_predictions = prediction() @@ -28,7 +28,8 @@ function obstacle() init_node("obstacle") loop_rate = Rate(10.0) - obstacle = Publisher("obs_prediction", prediction, queue_size=1)::RobotOS.Publisher{barc.msg.prediction} + obstacle = Publisher("obs_prediction", prediction, + queue_size=1)::RobotOS.Publisher{barc.msg.prediction} while !is_shutdown() obstacle_predictions.header.stamp = get_rostime() From e9c26701e361d00afd798ca709f57c43d8809ab3 Mon Sep 17 00:00:00 2001 From: lukasbrunke Date: Mon, 30 Apr 2018 10:05:07 -0700 Subject: [PATCH 168/183] Initial commit for running multiple agents. --- eval_sim/eval_data.jl | 122 ++ workspace/src/barc/CMakeLists.txt | 3 + .../src/barc/launch/barc_multi_sim.launch | 23 +- workspace/src/barc/launch/barc_sim.launch | 2 +- workspace/src/barc/launch/plotter.launch | 7 + workspace/src/barc/launch/test.launch | 65 + .../src/barc/launch/test_multiple.launch | 109 ++ workspace/src/barc/msg/prediction.msg | 5 +- workspace/src/barc/msg/selected_states.msg | 6 + workspace/src/barc/msg/theta.msg | 5 + workspace/src/barc/msg/xy_prediction.msg | 8 + .../src/.idea/codeStyles/codeStyleConfig.xml | 5 + .../src/barc/src/.idea/dictionaries/lukas.xml | 3 + .../inspectionProfiles/Project_Default.xml | 51 + workspace/src/barc/src/.idea/misc.xml | 4 + workspace/src/barc/src/.idea/modules.xml | 8 + workspace/src/barc/src/.idea/src.iml | 12 + workspace/src/barc/src/.idea/workspace.xml | 762 ++++++++++ workspace/src/barc/src/LMPC_node.jl | 34 +- workspace/src/barc/src/agent.jl | 1197 ++++++++++++++++ workspace/src/barc/src/agent.py | 172 +++ workspace/src/barc/src/agent_node.jl | 633 +++++++++ .../src/barc/src/barc_lib/LMPC/MPC_models.jl | 16 +- .../src/barc_lib/LMPC/coeffConstraintCost.jl | 16 +- .../src/barc/src/barc_lib/LMPC/functions.jl | 6 +- .../barc/src/barc_lib/LMPC/solveMpcProblem.jl | 7 +- workspace/src/barc/src/barc_lib/simModel.jl | 6 +- workspace/src/barc/src/barc_simulator_dyn.jl | 132 +- workspace/src/barc/src/config.jl | 234 ++++ workspace/src/barc/src/config_2.jl | 52 + workspace/src/barc/src/estimator.jl | 613 ++++++++ workspace/src/barc/src/estimator.py | 0 .../src/barc/src/low_level_controller.jl | 84 ++ workspace/src/barc/src/mpc_test.jl | 794 +++++++++++ workspace/src/barc/src/optimizer.jl | 1245 +++++++++++++++++ workspace/src/barc/src/plotter.py | 485 +++++++ workspace/src/barc/src/plotting_test.py | 242 ++++ workspace/src/barc/src/simple_simulator.jl | 264 ++++ workspace/src/barc/src/simulator.jl | 693 +++++++++ .../state_estimation_SensorKinematicModel.py | 29 +- ...ate_estimation_SensorKinematicModel_old.py | 316 +++++ workspace/src/barc/src/test_pf.jl | 82 ++ workspace/src/barc/src/test_sys_id.jl | 105 ++ workspace/src/barc/src/test_test.py | 242 ++++ workspace/src/barc/src/track.jl | 246 ++++ workspace/src/barc/src/track.json | 5 + workspace/src/barc/src/track.py | 199 +++ workspace/src/barc/src/transformations.jl | 228 +++ workspace/src/barc/src/transformations.py | 260 ++++ workspace/src/barc/src/view_car_trajectory.py | 56 +- workspace/src/barc/tracksoval.jld | Bin 0 -> 45184 bytes workspace/src/barc/trackstrack_3.jld | Bin 0 -> 23376 bytes 52 files changed, 9771 insertions(+), 122 deletions(-) create mode 100644 workspace/src/barc/launch/plotter.launch create mode 100644 workspace/src/barc/launch/test.launch create mode 100644 workspace/src/barc/launch/test_multiple.launch create mode 100644 workspace/src/barc/msg/selected_states.msg create mode 100644 workspace/src/barc/msg/theta.msg create mode 100644 workspace/src/barc/msg/xy_prediction.msg create mode 100644 workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml create mode 100644 workspace/src/barc/src/.idea/dictionaries/lukas.xml create mode 100644 workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml create mode 100644 workspace/src/barc/src/.idea/misc.xml create mode 100644 workspace/src/barc/src/.idea/modules.xml create mode 100644 workspace/src/barc/src/.idea/src.iml create mode 100644 workspace/src/barc/src/.idea/workspace.xml create mode 100644 workspace/src/barc/src/agent.jl create mode 100644 workspace/src/barc/src/agent.py create mode 100644 workspace/src/barc/src/agent_node.jl create mode 100644 workspace/src/barc/src/config.jl create mode 100644 workspace/src/barc/src/config_2.jl create mode 100644 workspace/src/barc/src/estimator.jl create mode 100644 workspace/src/barc/src/estimator.py create mode 100644 workspace/src/barc/src/low_level_controller.jl create mode 100644 workspace/src/barc/src/mpc_test.jl create mode 100644 workspace/src/barc/src/optimizer.jl create mode 100644 workspace/src/barc/src/plotter.py create mode 100644 workspace/src/barc/src/plotting_test.py create mode 100644 workspace/src/barc/src/simple_simulator.jl create mode 100644 workspace/src/barc/src/simulator.jl create mode 100644 workspace/src/barc/src/state_estimation_SensorKinematicModel_old.py create mode 100644 workspace/src/barc/src/test_pf.jl create mode 100644 workspace/src/barc/src/test_sys_id.jl create mode 100644 workspace/src/barc/src/test_test.py create mode 100644 workspace/src/barc/src/track.jl create mode 100644 workspace/src/barc/src/track.json create mode 100644 workspace/src/barc/src/track.py create mode 100644 workspace/src/barc/src/transformations.jl create mode 100644 workspace/src/barc/src/transformations.py create mode 100644 workspace/src/barc/tracksoval.jld create mode 100644 workspace/src/barc/trackstrack_3.jld diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl index 496caad0..a86a18a2 100644 --- a/eval_sim/eval_data.jl +++ b/eval_sim/eval_data.jl @@ -1745,6 +1745,128 @@ function create_track(w) #plot(x,y,x_l,y_l,x_r,y_r) end +function create_track_eval(w) + println("CREATING THE TRACK") + x = [0.0] # starting point + y = [0.0] + x_l = [0.0] # starting point + y_l = [w] + x_r = [0.0] # starting point + y_r = [-w] + ds = 0.03 + + theta = [0.0] + + # SOPHISTICATED TRACK + # add_curve(theta,30,0.0) + # add_curve(theta,60,-2*pi/3) + # add_curve(theta,90,pi) + # add_curve(theta,80,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,50,0.0) + # add_curve(theta,40,-pi/4) + # add_curve(theta,30,pi/4) + # add_curve(theta,20,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,25,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,28,0.0) + + # # SIMPLE track + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) + + # GOGGLE TRACK + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,-pi/6) + # add_curve(theta,30,pi/3) + # add_curve(theta,20,-pi/6) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + # SIMPLE GOGGLE TRACK + + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,pi/10) + # add_curve(theta,30,-pi/5) + # add_curve(theta,20,pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + add_curve(theta,60,0) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,40,pi/10) + add_curve(theta,60,-pi/5) + add_curve(theta,40,pi/10) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,75,0) + + # SIMPLE GOOGLE TRACK FOR 3110 + + # add_curve(theta,25,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,17,-pi/10) + # add_curve(theta,25,pi/5) + # add_curve(theta,17,-pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,32,0) + + # OVAL TRACK FOR TESTS IN VSD + + # add_curve(theta,80,0) + # add_curve(theta,110,-pi) + # add_curve(theta,160,0) + # add_curve(theta,110,-pi) + # add_curve(theta,80,0) + + # add_curve(theta,53,0) + # add_curve(theta,73,-pi) + # add_curve(theta,106,0) + # add_curve(theta,73,-pi) + # add_curve(theta,53,0) + + + # # SHORT SIMPLE track + # add_curve(theta,10,0) + # add_curve(theta,80,-pi) + # add_curve(theta,20,0) + # add_curve(theta,80,-pi) + # add_curve(theta,9,0) + + for i=1:length(theta) + push!(x, x[end] + cos(theta[i])*ds) + push!(y, y[end] + sin(theta[i])*ds) + push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) + push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) + push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) + push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) + end + track = cat(2, x, y, x_l, y_l, x_r, y_r) + #plot(track[:,1],track[:,2]) + return track + #plot(x,y,x_l,y_l,x_r,y_r) +end + function add_curve(theta::Array{Float64}, length::Int64, angle) d_theta = 0 curve = 2*sum(1:length/2)+length/2 diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index 3f405e3f..b7639c26 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -51,6 +51,9 @@ add_message_files( ECU.msg Vel_est.msg prediction.msg + selected_states.msg + xy_prediction.msg + theta.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_multi_sim.launch b/workspace/src/barc/launch/barc_multi_sim.launch index cbc097ef..dce7d84d 100755 --- a/workspace/src/barc/launch/barc_multi_sim.launch +++ b/workspace/src/barc/launch/barc_multi_sim.launch @@ -8,26 +8,27 @@ - + + - + - + - + - + - + @@ -40,9 +41,15 @@ - + + + + + + + - + diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch index b3ad47bc..7e5560d5 100644 --- a/workspace/src/barc/launch/barc_sim.launch +++ b/workspace/src/barc/launch/barc_sim.launch @@ -24,7 +24,7 @@ - + diff --git a/workspace/src/barc/launch/plotter.launch b/workspace/src/barc/launch/plotter.launch new file mode 100644 index 00000000..e6039f1f --- /dev/null +++ b/workspace/src/barc/launch/plotter.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/workspace/src/barc/launch/test.launch b/workspace/src/barc/launch/test.launch new file mode 100644 index 00000000..c2565b11 --- /dev/null +++ b/workspace/src/barc/launch/test.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_multiple.launch b/workspace/src/barc/launch/test_multiple.launch new file mode 100644 index 00000000..59e71ecb --- /dev/null +++ b/workspace/src/barc/launch/test_multiple.launch @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/msg/prediction.msg b/workspace/src/barc/msg/prediction.msg index 7d7a686a..a5577dae 100644 --- a/workspace/src/barc/msg/prediction.msg +++ b/workspace/src/barc/msg/prediction.msg @@ -3,4 +3,7 @@ Header header float64[] s float64[] ey float64[] epsi -float64[] v \ No newline at end of file +float64[] psidot +float64[] vx +float64[] vy +int32 current_lap \ No newline at end of file diff --git a/workspace/src/barc/msg/selected_states.msg b/workspace/src/barc/msg/selected_states.msg new file mode 100644 index 00000000..4eb73937 --- /dev/null +++ b/workspace/src/barc/msg/selected_states.msg @@ -0,0 +1,6 @@ +# This is a message to hold data of the selected states in x and y coords +Header header +float64[] x +float64[] y +float64[] s +float64[] ey \ No newline at end of file diff --git a/workspace/src/barc/msg/theta.msg b/workspace/src/barc/msg/theta.msg new file mode 100644 index 00000000..dfed806f --- /dev/null +++ b/workspace/src/barc/msg/theta.msg @@ -0,0 +1,5 @@ +# This is a message to communicate the agent's theta parameters +Header header +float64[] theta_vx +float64[] theta_vy +float64[] theta_psi_dot diff --git a/workspace/src/barc/msg/xy_prediction.msg b/workspace/src/barc/msg/xy_prediction.msg new file mode 100644 index 00000000..b1db55de --- /dev/null +++ b/workspace/src/barc/msg/xy_prediction.msg @@ -0,0 +1,8 @@ +# This is a message to communicate the agent's prediction +Header header +float64[] x +float64[] y +float64[] psi +float64[] acc +float64[] steering +int32 current_lap \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml b/workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml new file mode 100644 index 00000000..a55e7a17 --- /dev/null +++ b/workspace/src/barc/src/.idea/codeStyles/codeStyleConfig.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/dictionaries/lukas.xml b/workspace/src/barc/src/.idea/dictionaries/lukas.xml new file mode 100644 index 00000000..b165ff6b --- /dev/null +++ b/workspace/src/barc/src/.idea/dictionaries/lukas.xml @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml b/workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 00000000..2ac8da62 --- /dev/null +++ b/workspace/src/barc/src/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,51 @@ + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/misc.xml b/workspace/src/barc/src/.idea/misc.xml new file mode 100644 index 00000000..7ba73c25 --- /dev/null +++ b/workspace/src/barc/src/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/modules.xml b/workspace/src/barc/src/.idea/modules.xml new file mode 100644 index 00000000..f669a0e5 --- /dev/null +++ b/workspace/src/barc/src/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/src.iml b/workspace/src/barc/src/.idea/src.iml new file mode 100644 index 00000000..e98082ab --- /dev/null +++ b/workspace/src/barc/src/.idea/src.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/workspace/src/barc/src/.idea/workspace.xml b/workspace/src/barc/src/.idea/workspace.xml new file mode 100644 index 00000000..9abdd1ab --- /dev/null +++ b/workspace/src/barc/src/.idea/workspace.xml @@ -0,0 +1,762 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + kinematic + set + un + agent + reference_input + adversarial + s_to_xy + s_tO_xy + xy_to_s + loop_rate + P + current_reference + reference + yaw0 + dt + Localization + ll + param + Locali + pub + 0.125 + psiDot + warn + unwrap + modelParams + u_lb + + + + + + + + + + + + + + + + + + Buildout + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +