def simu():
global action, car
rospy.init_node('simu', anonymous=True)
rate = rospy.Rate(1 / dt)
rospy.loginfo("simulator node starts")
rospy.Subscriber('/vehicle/cmd_vel_stamped',
TwistStamped,
cmd_vel_stampedCallback,
queue_size=1)
rospy.Subscriber('/vehicle/steering_cmd',
SteeringCmd,
steering_cmdCallback,
queue_size=1)
pub1 = rospy.Publisher('/vehicle/steering_report',
SteeringReport,
queue_size=1)
pub2 = rospy.Publisher('state_estimate', state_Dynamic, queue_size=1)
steering_report = SteeringReport()
state_report = state_Dynamic()
while (rospy.is_shutdown() != 1):
car.simulate(action)
steering_report.steering_wheel_angle = car.state[6] * car.steeringRatio
state_report.vx = car.state[3]
state_report.vy = car.state[4] - car.state[5] * car.vhMdl[1]
state_report.X = car.state[0] - cos(car.state[2]) * car.vhMdl[1]
state_report.Y = car.state[1] - sin(car.state[2]) * car.vhMdl[1]
state_report.psi = car.state[2]
state_report.wz = car.state[5]
pub1.publish(steering_report)
pub2.publish(state_report)
rate.sleep()
def simu():
global action
rospy.init_node('simu', anonymous=True)
time.sleep(5)
rate = rospy.Rate(1 / dt)
rospy.loginfo("simulator node starts")
state = [558586.699614032, 4196501.20232720, 1.25432777404785, 5, 0, 0, 0]
#state = [0, 1, 0, 10, 0, 0, 0]
#state = [558641.76, 4196659.62, 1.25, 5, 0, 0, 0]
car.setState(state)
rospy.Subscriber('/vehicle/cmd_vel_stamped',
TwistStamped,
cmd_vel_stampedCallback,
queue_size=1)
rospy.Subscriber('/vehicle/steering_cmd',
SteeringCmd,
steering_cmdCallback,
queue_size=1)
pub1 = rospy.Publisher('/vehicle/steering_report',
SteeringReport,
queue_size=1)
pub2 = rospy.Publisher('state_estimate', state_Dynamic, queue_size=1)
# srv = Server(DynamicParamConfig, errorcallback)
steering_report = SteeringReport()
state_report = state_Dynamic()
while (rospy.is_shutdown() != 1):
car.simulate(action)
steering_report.steering_wheel_angle = car.state[6] * car.steeringRatio
state_report.vx = car.state[3]
state_report.vy = car.state[4]
state_report.X = car.state[0]
state_report.Y = car.state[1]
state_report.psi = car.state[2]
state_report.wz = car.state[5]
pub1.publish(steering_report)
pub2.publish(state_report)
rate.sleep()
# define the initial states and timestep
ra_to_cg = 1.65
X = 0
Y = 0
vx = 0
vy = 0
psi = 0
X_ref = 0
Y_ref = 0
vx_ref = 0
vy_ref = 0
psi_ref = 0
stateEstimate_mark = True
Waypoints_mark = False
Waypoints_received = Waypoints()
state_received = state_Dynamic()
steering_ratio = 14.8
def stateEstimateCallback(data):
global X, Y, psi, vx, vy, stateEstimate_mark, state_received
X = data.X + cos(data.psi) * ra_to_cg
Y = data.Y + sin(psi) * ra_to_cg
vx = data.vx
vy = data.vy
psi = data.psi
stateEstimate_mark = True
state_received = data
def WaypointsCallback(data):
def state_estimation():
global Vx_meas, Yaw_meas, delta_meas
global start_X, start_Y, pub_flag
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('/xsens/imu_data', Imu, IMUCallback)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
SteeringReportCallback)
state_pub = rospy.Publisher('state_estimate', state_Dynamic, queue_size=10)
# set node rate
loop_rate = 100
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
# estimation variables for Luemberger observer
z_EKF = array([0., 0., 0., 0., 0., 0.])
z_EKF_prev = array([0., 0., 0., 0., 0., 0.])
states_est = array([0., 0., 0., 0., 0., 0.])
state_initialize = 0
state_est_obj = state_Dynamic()
# estimation variables for EKF
var_v = 1.0e-04
var_psi = 1.0e-06
var_ax = 1.0e-04
var_delta = 1.0e-04
var_noise = 1.0e-04
P = eye(6) # initial dynamics coveriance matrix
Q = diag(
array([var_ax, var_delta, var_noise, var_noise, var_noise,
var_noise])) # process noise coveriance matrix
R1 = diag(array([var_v, var_psi]))
while (rospy.is_shutdown() != 1):
if state_initialize == 0:
z_EKF = array([Vx_meas, 0, 0, 0, Yaw_meas, Yawrate_meas])
state_est = z_EKF
state_initialize = 1
else:
u_ekf = array([ax_meas, delta_meas])
w_ekf = array([0., 0., 0., 0., 0., 0.])
args = (u_ekf, vhMdl, trMdl, dt)
z_EKF_prev = z_EKF
y_ekf = array([Vx_meas, Yaw_meas])
v_ekf = array([0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withoutGPS, y_ekf, Q, R1, args)
state_est = z_EKF
state_est_obj.vx = state_est[0]
state_est_obj.vy = state_est[1]
state_est_obj.X = state_est[2]
state_est_obj.Y = state_est[3]
state_est_obj.psi = state_est[4]
state_est_obj.wz = state_est[5]
state_pub.publish(state_est_obj)
rate.sleep()
def state_estimation():
global Vx_meas, Yaw_meas, X_meas, Y_meas, delta_meas
global Yaw_initialize, GPS_initialize, GPS_read
global start_X, start_Y, pub_flag
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('/xsens/imu/data', Imu, IMUCallback)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
SteeringReportCallback)
rospy.Subscriber('current_pose_2D', Pose2D, CurrentPose2DCallback)
rospy.Subscriber('relative_quaternion', Quaternion,
RelativeOrientationCallback)
state_pub = rospy.Publisher('state_estimate', state_Dynamic, queue_size=10)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
# estimation variables for Luemberger observer
z_EKF = array([0., 0., 0., 0., 0., 0.])
z_EKF_prev = array([0., 0., 0., 0., 0., 0.])
states_est = array([0., 0., 0., 0., 0., 0.])
state_initialize = 0
state_est_obj = state_Dynamic()
# estimation variables for EKF
var_gps = 1.0e-05
var_v = 1.0e-04
var_psi = 1.0e-04
var_ax = 1.0e-03
var_ay = 1.0e-03
var_delta = 1.0e-02
var_noise = 1.0e-01
P = eye(6) # initial dynamics coveriance matrix
Q = diag(
array([var_ax, var_delta, var_noise, var_noise, var_noise,
var_noise])) # process noise coveriance matrix
R2 = diag(array([var_v, var_gps, var_gps,
var_psi])) # measurement noise coveriance matrix
R1 = diag(array([var_v, var_psi, var_ay]))
while (rospy.is_shutdown() != 1):
#print((Yaw_initialize, GPS_initialize))
if Yaw_initialize == 0 or GPS_initialize == 0:
rate.sleep()
continue
elif state_initialize == 0:
z_EKF = array([Vx_meas, 0, X_meas, Y_meas, Yaw_meas, Yawrate_meas])
state_est = z_EKF
state_initialize = 1
else:
factor = 1.0
if abs(Vx_meas) < 0.01:
factor = 0.0
u_ekf = array([ax_meas * factor, delta_meas * factor])
w_ekf = array([0., 0., 0., 0., 0., 0.])
args = (u_ekf, vhMdl, trMdl, dt)
z_EKF_prev = z_EKF
if GPS_read == 0:
y_ekf = array([
Vx_meas * factor,
Yaw_meas * factor + z_EKF[4] * (1. - factor),
ay_meas * factor
])
v_ekf = array([0., 0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withoutGPS, y_ekf, Q, R1, args)
else:
y_ekf = array([Vx_meas, X_meas, Y_meas, Yaw_meas])
v_ekf = array([0., 0., 0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withGPS, y_ekf, Q, R2, args)
GPS_read = 0
state_est = z_EKF
state_est_obj.vx = state_est[0]
state_est_obj.vy = state_est[1]
state_est_obj.X = state_est[2]
state_est_obj.Y = state_est[3]
state_est_obj.psi = state_est[4]
state_est_obj.wz = state_est[5]
if Vx_meas == 0:
state_est_obj.vx = 0
state_est_obj.vy = 0
state_pub.publish(state_est_obj)
rate.sleep()
def state_estimation():
global Vx_meas, Yaw_meas, delta_meas, ax_meas, ay_meas
global start_X, start_Y, pub_flag, steering_received, imu_received
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('/xsens/imu/data', Imu, IMUCallback)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
SteeringReportCallback)
state_pub = rospy.Publisher('state_estimate', state_Dynamic, queue_size=1)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
# estimation variables for Luemberger observer
z_EKF = array([0., 0., 0., 0., 0., 0.])
z_EKF_prev = array([0., 0., 0., 0., 0., 0.])
states_est = array([0., 0., 0., 0., 0., 0.])
state_initialize = 0
state_est_obj = state_Dynamic()
# estimation variables for EKF
var_v = 1.0e-04
var_psi = 1.0e-03
var_ax = 1.0e-02
var_ay = 1.0e-02
var_delta = 1.0e-02
var_noise = 1.0e-01
P = eye(6) # initial dynamics coveriance matrix
Q = diag(
array([var_ax, var_delta, var_noise, var_noise, var_noise,
var_noise])) # process noise coveriance matrix
R1 = diag(array([var_v, var_psi, var_ay]))
while (rospy.is_shutdown() != 1):
if steering_received and imu_received:
steering_received = False
imu_received = False
if state_initialize == 0:
#z_EKF = array([Vx_meas, 0, 0, 0, Yaw_meas, Yawrate_meas])
z_EKF = array([0, 0, 0, 0, Yaw_meas, 0])
state_est = z_EKF
state_initialize = 1
else:
factor = 1.0
if abs(Vx_meas) < 0.01:
factor = 0.0
while (Yaw_meas - state_est[4] > pi):
Yaw_meas -= 2 * pi
while (Yaw_meas - state_est[4] < -pi):
Yaw_meas += 2 * pi
u_ekf = array([ax_meas * factor, delta_meas * factor])
w_ekf = array([0., 0., 0., 0., 0., 0.])
args = (u_ekf, vhMdl, trMdl, dt)
z_EKF_prev = z_EKF
y_ekf = array([
Vx_meas * factor,
Yaw_meas * factor + z_EKF[4] * (1. - factor),
ay_meas * factor
])
v_ekf = array([0., 0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withoutGPS, y_ekf, Q, R1, args)
z_EKF[4] = z_EKF[4] % (2 * pi)
if z_EKF[4] > pi:
z_EKF[4] -= 2 * pi
state_est = z_EKF
state_est_obj.vx = state_est[0]
state_est_obj.vy = state_est[1]
state_est_obj.X = state_est[2]
state_est_obj.Y = state_est[3]
state_est_obj.psi = state_est[4]
state_est_obj.wz = state_est[5]
state_pub.publish(state_est_obj)
rate.sleep()
