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)