def __init__(self):
# init node subs pubs
rospy.init_node('vehiclemodel', anonymous=True)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.dynamicparamsub = rospy.Subscriber("dynamic_vehicle_params",
DynamicVehicleParams,
self.dynamicparams_callback)
self.pathlocalsub = rospy.Subscriber("pathlocal", PathLocal,
self.pathlocal_callback)
self.statepub = rospy.Publisher('state', State, queue_size=10)
self.dt = 0.1
self.rate = rospy.Rate(1.0 / self.dt)
#self.rate = rospy.Rate(2)
# set static params
self.setStaticParams()
# set init state
self.setInitState()
#self.ctrl_cmd = CtrlCmd()
self.dp = DynamicVehicleParams()
self.pathlocal = PathLocal()
self.trajstar = Trajectory()
# wait for messages before entering main loop
while (not self.pathlocal.s):
print("waiting for pathlocal")
self.statepub.publish(self.state)
self.rate.sleep()
while (not self.sp.Iz):
print("waiting for static params")
self.statepub.publish(self.state)
self.rate.sleep()
while (not self.trajstar.Fyf):
print("waiting for trajstar")
self.statepub.publish(self.state)
self.rate.sleep()
# main loop
while not rospy.is_shutdown():
# get new vehicle state
self.propagateVehicle()
self.state.header.stamp = rospy.Time.now()
self.statepub.publish(self.state)
# end of main loop
self.rate.sleep()
def __init__(self):
# init node subs pubs
rospy.init_node('ctrl_interface', anonymous=True)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.vehicle_out_sub = rospy.Subscriber("vehicle_out", VehicleOut,
self.vehicle_out_callback)
self.vehicleinpub = rospy.Publisher('vehicle_in',
VehicleIn,
queue_size=10)
self.rate = rospy.Rate(100)
# set static vehicle params
self.setStaticParams()
# init msgs
self.vehicle_in = VehicleIn()
self.vehicle_out = VehicleOut()
self.trajstar = Trajectory()
# wait for messages before entering main loop # tmp commented out!!
while (not self.trajstar.Fyf):
print("waiting for trajstar")
self.rate.sleep()
# main loop
while not rospy.is_shutdown():
# compute delta corresponding to Fyf request (linear tire, Rajamani)
Fyf_request = self.trajstar.Fyf[0]
alpha_f = Fyf_request / (2 * self.sp.Cr)
psidot = self.vehicle_out.psidot
vx = self.vehicle_out.vx
vy = self.vehicle_out.vy
if (self.vehicle_out.vx > 0.1):
delta = alpha_f + np.arctan2(vy + self.sp.lf * psidot, vx)
else:
delta = 0
self.vehicle_in.delta = delta
#self.vehicle_in.Fyf = self.trajstar.Fyf[0]
self.vehicle_in.Fx = self.trajstar.Fx[0]
self.vehicle_in.header.stamp = rospy.Time.now()
self.vehicleinpub.publish(self.vehicle_in)
def __init__(self):
# params
self.robot_name = rospy.get_param('/robot_name')
# init node subs pubs
rospy.init_node('ctrl_interface', anonymous=True)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.pathlocalsub = rospy.Subscriber("pathlocal", Path,
self.pathlocal_callback)
self.state_sub = rospy.Subscriber("/state", State, self.state_callback)
self.ctrlmodesub = rospy.Subscriber("ctrl_mode", Int16,
self.ctrl_mode_callback)
self.vehicleinpub = rospy.Publisher('/fssim/cmd', Cmd, queue_size=10)
self.lhptpub = rospy.Publisher('/lhpt_vis', Marker, queue_size=1)
self.vx_errorpub = rospy.Publisher('/vx_error_vis',
Float32,
queue_size=1)
self.rate = rospy.Rate(100)
# set static vehicle params
self.setStaticParams()
# init msgs
self.state = State()
self.vehicle_in = Cmd()
self.trajstar = Trajectory()
self.pathlocal = Path()
self.ctrl_mode = 0 # 0: stop, 1: cruise_ctrl, 2: tamp
self.trajstar_received = False
self.pathlocal_received = False
self.state_received = False
# ctrl errors
self.vx_error = Float32()
# get cc setpoint
self.cc_vxref = rospy.get_param('/cc_vxref')
self.cc_dref = rospy.get_param('/cc_dref')
# delay sim variable
self.delta_out_FIFO = []
# misc vars
self.delta_out_last = 0
self.dc_out_last = 0
# wait for messages before entering main loop
while (not self.state_received):
rospy.loginfo_throttle(1, "waiting for state")
self.rate.sleep()
while (not self.pathlocal_received):
rospy.loginfo_throttle(1, "waiting for pathlocal")
self.rate.sleep()
while (self.ctrl_mode == 0):
rospy.loginfo_throttle(1,
"waiting for activation from exp manager")
self.rate.sleep
# main loop
while not rospy.is_shutdown():
if (self.ctrl_mode == 0
): # STOP (set by exp manager if outside of track)
if (self.state.vx > 0.1):
rospy.loginfo_throttle(1, "in stop mode")
delta_out = self.delta_out_last
dc_out = -200000 #self.dc_out_last
else:
delta_out = 0
dc_out = 0
elif (self.ctrl_mode == 1): # CRUISE CTRL
delta_out, dc_out, Xlh, Ylh = self.cc_ctrl()
elif (self.ctrl_mode == 2): # TAMP
while (not self.trajstar_received):
print("waiting for trajstar, stopping")
delta_out = 0
dc_out = 0
self.rate.sleep()
delta_out, dc_out, Xlh, Ylh = self.tamp_ctrl()
else:
print "invalid ctrl_mode! ctrl_mode = ", self.ctrl_mode
# publish ctrl cmd
self.vehicle_in.delta = delta_out
#print "dc_out published = ", dc_out
self.vehicle_in.dc = dc_out
self.vehicleinpub.publish(self.vehicle_in)
# publish tuning info
self.vx_errorpub.publish(self.vx_error)
if (self.ctrl_mode in [1, 2]):
m = self.getlhptmarker(Xlh, Ylh)
self.lhptpub.publish(m)
# store latest controls
self.delta_out_last = delta_out
self.dc_out_last = dc_out
self.rate.sleep()
def __init__(self):
# init node subs pubs
rospy.init_node('ctrl_interface', anonymous=True)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.vehicle_out_sub = rospy.Subscriber(
"/fssim/base_pose_ground_truth", fssimState,
self.vehicle_out_callback)
self.vehicleinpub = rospy.Publisher('/fssim/cmd', Cmd, queue_size=10)
self.rate = rospy.Rate(10) # TMP!
# set static vehicle params
self.setStaticParams()
# init msgs
self.vehicle_in = Cmd()
self.vehicle_out = fssimState()
self.trajstar = Trajectory()
# misc
self.printcounter = 0
# wait for messages before entering main loop # tmp commented out!!
while (not self.trajstar.Fyf):
print("waiting for trajstar")
self.rate.sleep()
# main loop
while not rospy.is_shutdown():
# compute delta corresponding to Fyf request (linear tire, Rajamani)
Fyf_request = self.trajstar.Fyf[0]
alpha_f = Fyf_request / (2 * self.Cf)
psidot = self.vehicle_out.r
vx = self.vehicle_out.vx
vy = self.vehicle_out.vy
# compute angle of velocity vector at center of front axle
if (vx < 1.0):
theta_Vf = np.arctan2(vy + self.lf * psidot, 1.0)
else:
theta_Vf = np.arctan2(vy + self.lf * psidot, vx)
# compute controls
delta_out = 1.0 * (theta_Vf + alpha_f)
dc_out = 0.00006 * self.trajstar.Fx[0]
# prints
print ""
print "params"
print "lf = ", str(self.lf)
print "Cf ", str(self.Cf)
print ""
print "state vars"
print "vx = ", str(vx)
print "vy = ", str(vy)
print "psidot = ", str(psidot)
print ""
print "computed vars"
print "theta_Vf = ", str(theta_Vf)
print "Fyf_request = ", str(Fyf_request)
print "alpha_f_request = ", str(alpha_f)
print "delta_out = ", str(delta_out)
print "dc_out = ", str(dc_out)
# publish ctrl cmd
self.vehicle_in.delta = delta_out
self.vehicle_in.dc = dc_out
self.vehicleinpub.publish(self.vehicle_in)
def __init__(self):
# timing params
self.dt_sim = 0.01 # timestep of the simulation
self.t_activate = rospy.get_param('/t_activate')
self.t_final = rospy.get_param('/t_final')
# pop-up scenario params
self.s_ego_at_popup = rospy.get_param('/s_ego_at_popup')
self.s_obs_at_popup = rospy.get_param('/s_obs_at_popup')
self.d_obs_at_popup = rospy.get_param('/d_obs_at_popup')
# track params
self.track_name = rospy.get_param('/track_name')
self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
self.mu_segment_values = rospy.get_param('/mu_segment_values')
self.N_mu_segments = len(self.s_begin_mu_segments)
self.mu_segment_idx = 0
# vehicle params
self.vehicle_width = rospy.get_param('/car/kinematics/l_width')
# algo params (from acado)
N = 40
dt_algo = 0.1
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathglobalsub = rospy.Subscriber("pathglobal", Path,
self.pathglobal_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.carinfosub = rospy.Subscriber("/fssim/car_info", CarInfo,
self.fssim_carinfo_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.obspub = rospy.Publisher('/obs', Obstacles, queue_size=1)
self.obsvispub = rospy.Publisher('/obs_vis', Marker, queue_size=1)
self.tireparampub = rospy.Publisher('/tire_params',
TireParams,
queue_size=1)
self.ctrl_mode_pub = rospy.Publisher('/ctrl_mode', Int16, queue_size=1)
self.statetextmarkerpub = rospy.Publisher('/state_text_marker',
Marker,
queue_size=1)
# init logging vars
self.explog_activated = False
self.explog_iterationcounter = 0
self.stored_pathglobal = False
self.stored_trajstar = False
self.stored_trajcl = False
self.explog_saved = False
# init misc internal variables
self.pathglobal = Path()
self.received_pathglobal = False
self.state = State()
self.received_state = False
self.trajstar = Trajectory()
self.received_trajstar = False
self.fssim_carinfo = CarInfo()
while (not self.received_pathglobal):
print("waiting for pathglobal")
time.sleep(self.dt_sim * 100)
# init experiment variables
self.scenario_id = rospy.get_param('/scenario_id')
self.tireparams = TireParams()
self.obs = Obstacles()
self.obs.s = [self.s_obs_at_popup]
self.obs.d = [self.d_obs_at_popup]
self.obs.R = [0.5]
wiggleroom = 0.5
self.obs.Rmgn = [
0.5 * self.obs.R[0] + 0.5 * self.vehicle_width + wiggleroom
]
Xobs, Yobs = ptsFrenetToCartesian(np.array(self.obs.s), \
np.array(self.obs.d), \
np.array(self.pathglobal.X), \
np.array(self.pathglobal.Y), \
np.array(self.pathglobal.psi_c), \
np.array(self.pathglobal.s))
self.ctrl_mode = 0 # # 0: stop, 1: cruise_ctrl, 2: tamp
# Main loop
#print 'running experiment: '
#print 'track: ', self.track_name
self.exptime = 0
while (not rospy.is_shutdown()) and self.exptime < self.t_final:
if (self.exptime >= self.t_activate):
rospy.loginfo_throttle(1, "Running experiment")
# HANDLE TRACTION IN SIMULATION
s_ego = self.state.s % self.s_lap
for i in range(self.N_mu_segments - 1):
if (self.s_begin_mu_segments[i] <= s_ego <=
self.s_begin_mu_segments[i + 1]):
self.mu_segment_idx = i
break
if (s_ego >= self.s_begin_mu_segments[-1]):
self.mu_segment_idx = self.N_mu_segments - 1
mu = self.mu_segment_values[self.mu_segment_idx]
#print "s_ego = ", s_ego
#print "state.s = ", self.state.s
#print "self.N_mu_segments = ", self.N_mu_segments
#print "mu_segment_idx = ", self.mu_segment_idx
#print "mu in this section = ", self.mu_segment_values[self.mu_segment_idx]
# set tire params of sim vehicle
if (0.0 <= mu < 0.3): # ice
B = 4.0
C = 2.0
D = mu
E = 1.0
elif (0.3 <= mu < 0.5): # snow
B = 5.0
C = 2.0
D = mu
E = 1.0
elif (0.5 <= mu < 0.9): # wet
B = 12.0
C = 2.3
D = mu
E = 1.0
elif (0.9 <= mu < 1.5): # dry
B = 10.0
C = 1.9
D = mu
E = 0.97
elif (1.5 <= mu <
2.5): # dry + racing tires (gotthard default)
B = 12.56
C = 1.38
D = mu
E = 1.0
else:
rospy.loginfo_throttle(1, "Faulty mu value in exp manager")
self.tireparams.tire_coefficient = 1.0
#self.tireparams.B = 10
#self.tireparams.C = 1.9
#self.tireparams.D = -mu
#self.tireparams.E = 1.0
self.tireparams.B = B
self.tireparams.C = C
self.tireparams.D = -D
self.tireparams.E = E
self.tireparams.header.stamp = rospy.Time.now()
self.tireparampub.publish(self.tireparams)
# POPUP SCENARIO
if (self.scenario_id == 1):
self.ctrl_mode = 1 # cruise control
m_obs = self.getobstaclemarker(Xobs, Yobs, self.obs.R[0])
m_obs.color.a = 0.3 # transparent before detect
if (self.state.s >= self.s_ego_at_popup):
self.obspub.publish(self.obs)
m_obs.color.a = 1.0 # non-transparent after detect
self.ctrl_mode = 2 # tamp
self.obsvispub.publish(m_obs)
# REDUCED MU TURN
elif (self.scenario_id == 2):
self.ctrl_mode = 1 # pp cruise control from standstill
if (self.state.vx > 5): # todo
self.ctrl_mode = 2 # tamp cruise control when we get up to speed
# RACING
else:
self.ctrl_mode = 2 # tamp
# SEND STOP IF EXIT TRACK
dlb = np.interp(self.state.s, self.pathglobal.s,
self.pathglobal.dlb)
dub = np.interp(self.state.s, self.pathglobal.s,
self.pathglobal.dub)
if (self.state.d < dlb or self.state.d > dub):
self.ctrl_mode = 0 # stop
self.ctrl_mode_pub.publish(self.ctrl_mode)
# publish text marker (state info)
state_text = "vx: " + "%.3f" % self.state.vx + "\n" \
"mu: " + "%.3f" % mu
self.statetextmarkerpub.publish(self.gettextmarker(state_text))
# save data for plot
filename = "/home/larsvens/ros/tamp__ws/src/saarti/common/logs/explog_latest.npy" # todo get from param
self.s_begin_log = 195 # 190 todo get from param
if (self.state.s >= self.s_begin_log
and not self.explog_activated):
print "STARTED EXPLOG"
t_start_explog = copy.deepcopy(self.exptime)
self.explog_activated = True
# store planned traj
self.trajstar_dict = {
"X": np.array(self.trajstar.X),
"Y": np.array(self.trajstar.Y),
"psi": np.array(self.trajstar.psi),
"s": np.array(self.trajstar.s),
"d": np.array(self.trajstar.d),
"deltapsi": np.array(self.trajstar.deltapsi),
"psidot": np.array(self.trajstar.psidot),
"vx": np.array(self.trajstar.vx),
"vy": np.array(self.trajstar.vy),
"ax": np.array(self.trajstar.ax),
"ay": np.array(self.trajstar.ay),
"Fyf": np.array(self.trajstar.Fyf),
"Fxf": np.array(self.trajstar.Fxf),
"Fyr": np.array(self.trajstar.Fyr),
"Fxr": np.array(self.trajstar.Fxr),
"Fzf": np.array(self.trajstar.Fzf),
"Fzr": np.array(self.trajstar.Fzr),
"kappac": np.array(self.trajstar.kappac),
"Cr": np.array(self.trajstar.Cr),
"t": np.arange(0, (N + 1) * dt_algo, dt_algo),
}
self.stored_trajstar = True
# initialize vars for storing CL traj
self.X_cl = []
self.Y_cl = []
self.psi_cl = []
self.s_cl = []
self.d_cl = []
self.deltapsi_cl = []
self.psidot_cl = []
self.vx_cl = []
self.vy_cl = []
self.ax_cl = []
self.ay_cl = []
self.t_cl = []
self.Fyf_cl = []
self.Fyr_cl = []
self.Fx_cl = []
if (self.state.s >= self.s_begin_log and self.explog_activated
and self.exptime >= t_start_explog +
self.explog_iterationcounter * dt_algo):
# build CL traj
self.X_cl.append(self.state.X)
self.Y_cl.append(self.state.Y)
self.psi_cl.append(self.state.psi)
self.s_cl.append(self.state.s)
self.d_cl.append(self.state.d)
self.deltapsi_cl.append(self.state.deltapsi)
self.psidot_cl.append(self.state.psidot)
self.vx_cl.append(self.state.vx)
self.vy_cl.append(self.state.vy)
self.ax_cl.append(self.state.ax)
self.ay_cl.append(self.state.ay)
self.t_cl.append(self.exptime)
self.Fyf_cl.append(self.fssim_carinfo.Fy_f_l +
self.fssim_carinfo.Fy_f_r)
self.Fyr_cl.append(self.fssim_carinfo.Fy_r)
self.Fx_cl.append(self.fssim_carinfo.Fx)
# store CL traj as dict
if (self.explog_iterationcounter == N
): # store N+1 values, same as trajstar
self.trajcl_dict = {
"X": np.array(self.X_cl),
"Y": np.array(self.Y_cl),
"psi": np.array(self.psi_cl),
"s": np.array(self.s_cl),
"d": np.array(self.d_cl),
"deltapsi": np.array(self.deltapsi_cl),
"psidot": np.array(self.psidot_cl),
"vx": np.array(self.vx_cl),
"vy": np.array(self.vy_cl),
"ax": np.array(self.ax_cl),
"ay": np.array(self.ay_cl),
"t": np.array(self.t_cl),
"Fyf": np.array(self.Fyf_cl),
"Fyr": np.array(self.Fyr_cl),
"Fx": np.array(self.Fx_cl),
}
self.stored_trajcl = True
self.explog_iterationcounter += 1
# save explog
# debug
#print "stored_pathglobal: ", self.stored_pathglobal
#print "stored_trajstar: ", self.stored_trajstar
#print "stored_trajcl: ", self.stored_trajcl
if (self.stored_pathglobal and self.stored_trajstar
and self.stored_trajcl and not self.explog_saved):
explog = {
"pathglobal": self.pathglobal_dict,
"trajstar": self.trajstar_dict,
"trajcl": self.trajcl_dict,
}
np.save(filename, explog)
self.explog_saved = True
print "SAVED EXPLOG"
else: # not reached activation time
rospy.loginfo_throttle(
1, "Experiment starting in %i seconds" %
(self.t_activate - self.exptime))
# handle exptime
self.exptime += self.dt_sim
msg = Clock()
t_rostime = rospy.Time(self.exptime)
msg.clock = t_rostime
#self.clockpub.publish(msg)
time.sleep(self.dt_sim)
print 'simulation finished'
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)
def trajhat_publisher():
rospy.init_node('trajhat_publisher', anonymous=True)
traj_pub = rospy.Publisher('tmp_trajhat', Trajectory, queue_size=10)
rate = rospy.Rate(10) # 10hz
# load .mat file
filename = '/home/larsvens/ros/tamp_ws/src/common/data/trajhat_example.mat'
mat = sio.loadmat(filename, struct_as_record=False, squeeze_me=True)
while not rospy.is_shutdown():
trajhat = Trajectory()
trajhat.header.stamp = rospy.Time.now()
trajhat.t = mat['traj_hat'].t
trajhat.kappac = mat['traj_hat'].kappa_c
trajhat.s = mat['traj_hat'].s
trajhat.d = mat['traj_hat'].d
trajhat.X = mat['traj_hat'].X
trajhat.Y = mat['traj_hat'].Y
trajhat.deltapsi = mat['traj_hat'].deltapsi
trajhat.psi = mat['traj_hat'].psi
trajhat.psidot = mat['traj_hat'].psidot
trajhat.vx = mat['traj_hat'].vx
trajhat.vy = mat['traj_hat'].vy
trajhat.ax = mat['traj_hat'].ax
trajhat.ay = mat['traj_hat'].ay
trajhat.Fyf = mat['traj_hat'].Fyf
trajhat.Fxf = mat['traj_hat'].Fxf
trajhat.Fyr = mat['traj_hat'].Fyr
trajhat.Fxr = mat['traj_hat'].Fxr
trajhat.Fx = mat['traj_hat'].Fx
trajhat.alphaf = mat['traj_hat'].alphaf
trajhat.alphar = mat['traj_hat'].alphar
trajhat.Crtilde = mat['traj_hat'].Crtilde
trajhat.delta = mat['traj_hat'].delta
trajhat.valid = mat['traj_hat'].valid
trajhat.coll_cost = mat['traj_hat'].coll_cost
trajhat.cost = mat['traj_hat'].cost
traj_pub.publish(trajhat)
# end of loop
rate.sleep()
import rospy
import time
import math
import os
import numpy as np
import matplotlib.pyplot as plt
import sumo.vehicleControl as vehicleControl
from common.msg import Pose
from common.msg import Trajectory
from common.msg import DynamicObstacle
from common.msg import ObstacleMap
CAR_WIDTH = 3
trajectory = Trajectory()
trajectory_ready = False
def callback_trajectory(msg):
global trajectory
trajectory = msg
global trajectory_ready
trajectory_ready = True
#print "callback traj:", trajectory
def init_sim():
vehicleControl.init()
vehicleControl.init_vehicle()
def __init__(self):
# params
self.dt = 0.01 # timestep of the simulation
t_final = rospy.get_param('/t_final')
do_liveplot = rospy.get_param('/do_liveplot')
save_logfile = rospy.get_param('/save_logfile')
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#rospy.init_node('experiment_manager', anonymous=True, disable_signals=True)
self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathlocalsub = rospy.Subscriber("pathlocal", Path,
self.pathlocal_callback)
self.obstaclesub = rospy.Subscriber("obstacles", Obstacles,
self.obstacles_callback)
self.trajhatsub = rospy.Subscriber("trajhat", Trajectory,
self.trajhat_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.dynamicparamsub = rospy.Subscriber("dynamic_vehicle_params",
DynamicVehicleParams,
self.dynamicparams_callback)
# load global path
self.loadPathGlobalFromFile()
# set static vehicle params
self.setStaticParams()
# init internal variables
self.counter = 0 # use this to reduce plot update rate
self.pathlocal = Path()
self.obstacles = Obstacles()
self.trajhat = Trajectory()
self.trajstar = Trajectory()
self.state = State()
self.dp = DynamicVehicleParams()
# init lists for logging
self.tvec = []
self.states = []
if (do_liveplot):
# init plot window
plt.ion()
self.f, (self.a0, self.a1) = plt.subplots(
1, 2, gridspec_kw={'width_ratios': [3, 1]})
self.a0.axis("equal")
self.a1.axis("equal")
# Main loop
self.t = 0
while (not rospy.is_shutdown()) and self.t < t_final:
# store data from subscribed topics
if (save_logfile):
self.stack_data()
# handle simtime
self.t += self.dt
msg = Clock()
t_rostime = rospy.Time(self.t)
msg.clock = t_rostime
self.clockpub.publish(msg)
if (do_liveplot):
self.liveplot()
slowdown_factor = 1
else:
slowdown_factor = 1
print 'simtime t =', t_rostime.to_sec()
time.sleep(self.dt * slowdown_factor)
print 'simulation finished'
if (save_logfile):
self.save_log()
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)