def _publishClock(self, clockRate):
print 'Starting to publish simulated time on /clock and setting /use_sim_time to true.'
print 'Simulated time is running at %fx speed.' % clockRate
clockPublisher = rospy.Publisher( "/clock", Clock, queue_size=1 )
if self.timingMode == LogfilePublisher.TimingMode.use_timestamps:
while isinf(self._earliestTimestamp):
time.sleep(0.01)
currentSecs = self._earliestTimestamp - 0.5
else:
currentSecs = time.time()
lastRealTime = time.time()
while not rospy.is_shutdown():
currentRealTime = time.time()
elapsedRealTime = currentRealTime - lastRealTime
lastRealTime = currentRealTime
if not self._paused or self._singleStep:
currentSecs += elapsedRealTime * clockRate
clock = Clock()
clock.clock = rospy.Time.from_sec(currentSecs)
try:
clockPublisher.publish(clock)
except Exception:
print 'Publisher closed'
time.sleep(0.01)
def default(self, ci='unused'):
""" Publish the simulator clock as a Clock ROS message """
msg = Clock()
msg.clock = rospy.Time.from_sec(self._sim_time)
self._sim_time += self._sim_step
self.publish(msg)
def set_time(TestStorage, value):
wanted_time = rospy.Time(value)
c = Clock()
c.clock = wanted_time
while rospy.Time.now() != wanted_time:
TestStorage.pub.publish(c)
time.sleep(0.01)
def post_clock(self, component_instance):
""" Publish the simulator clock as a Clock ROS msg
"""
msg = Clock()
msg.clock = rospy.Time.from_sec(self._sim_time)
self._sim_time += self._sim_step
for topic in self._topics:
# publish the message on the correct topic
if str(topic.name) == str("/" + component_instance.blender_obj.name):
topic.publish(msg)
def _publishClock(self, clockRate):
print 'Starting to publish simulated time on /clock and setting /use_sim_time to true.'
print 'Simulated time is running at %fx speed.' % clockRate
clockPublisher = rospy.Publisher( "/clock", Clock )
currentSecs = time.time()
while not rospy.is_shutdown():
if not self._paused or self._singleStep:
currentSecs += 0.01 * clockRate
clock = Clock()
clock.clock = rospy.Time.from_sec(currentSecs)
try:
clockPublisher.publish(clock)
except Exception:
print 'Publisher closed'
time.sleep(0.01)
def StreamLidarSensor(self, request, context):
"""
Takes in a gRPC LidarStreamingRequest containing
all the data needed to create and publish a PointCloud2
ROS message.
"""
pointcloud_msg = PointCloud2()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.from_sec(request.timeInSeconds)
header.frame_id = "velodyne"
pointcloud_msg.header = header
pointcloud_msg.height = request.height
pointcloud_msg.width = request.width
fields = request.fields
# Set PointCloud[] fields in pointcloud_msg
for i in range(len(fields)):
pointcloud_msg.fields.append(PointField())
pointcloud_msg.fields[i].name = fields[i].name
pointcloud_msg.fields[i].offset = fields[i].offset
pointcloud_msg.fields[i].datatype = fields[i].datatype
pointcloud_msg.fields[i].count = fields[i].count
pointcloud_msg.is_bigendian = request.isBigEndian
pointcloud_msg.point_step = request.point_step
pointcloud_msg.row_step = request.row_step
pointcloud_msg.data = request.data
pointcloud_msg.is_dense = request.is_dense
self.lidar_pub.publish(pointcloud_msg)
# TODO: This does not belong in this RPC implementation, should be
# moved to own or something like that.
sim_clock = Clock()
sim_clock.clock = rospy.Time.from_sec(request.timeInSeconds)
self.clock_pub.publish(sim_clock)
return sensor_streaming_pb2.LidarStreamingResponse(success=True)
def step(self, ms):
"""Call this method on each step."""
if self.__joint_state_publisher:
self.__joint_state_publisher.publish()
if self.__device_manager:
self.__device_manager.step()
if self.robot is None or self.get_parameter('synchronization').value:
return
# Robot step
if self.robot.step(ms) < 0.0:
del self.robot
self.robot = None
sys.exit(0)
# Update time
msg = Clock()
msg.clock = Time(seconds=self.robot.getTime()).to_msg()
self.__clock_publisher.publish(msg)
def talker():
pub = rospy.Publisher('clock', Clock, queue_size=10)
rospy.init_node('clock_server', anonymous=True)
print "publishing.."
# how much faster then reallity?
mult_factor = rospy.get_param("/ctm/clock_mult", 10)
# in ms
rate = 10
# in s
cur_time = 10000.0
while not rospy.is_shutdown():
c = Clock()
c.clock = rospy.Time.from_sec(cur_time)
pub.publish(c)
time.sleep(rate / 1000.0)
cur_time += rate / 1000.0 * mult_factor
print "done."
def talker():
pub = rospy.Publisher('clock', Clock, queue_size=10)
rospy.init_node('clock_server', anonymous=True)
print "publishing.."
# how much faster then reallity?
mult_factor = rospy.get_param("/ctm/clock_mult", 10)
# in ms
rate = 10
# in s
cur_time = 10000.0
while not rospy.is_shutdown():
c = Clock()
c.clock = rospy.Time.from_sec(cur_time)
pub.publish(c)
time.sleep(rate / 1000.0)
cur_time += rate / 1000.0 * mult_factor
print "done."
def odom_callback(self, event):
self.update_loc()
msg = Odometry()
msg.pose.pose.position.x = self.cLoc.x
msg.pose.pose.position.y = self.cLoc.y
msg.pose.pose.position.z = self.goal_altitude
msg.pose.pose.orientation.x = 0.0
msg.pose.pose.orientation.y = 0.0
msg.pose.pose.orientation.z = 0.0
msg.pose.pose.orientation.w = 1.0
msg.twist.twist.linear.x = self.cruising_speed
msg.twist.twist.linear.y = 0.0
msg.twist.twist.linear.z = 0.0
msg.twist.twist.angular.x = 0.0
msg.twist.twist.angular.y = 0.0
msg.twist.twist.angular.z = 0.0
self.pub_odom.publish(msg)
clk = Clock()
clk.clock = rospy.get_rostime()
self.pub_clock.publish(clk)
def RunClock():
time.sleep(5)
rospy.init_node('emerald_clock_node', anonymous=False)
systemTime = rospy.Publisher('/emerald_ai/helper/clock/system',
String,
queue_size=2)
rosTime = rospy.Publisher('/emerald_ai/helper/clock/ros',
Clock,
queue_size=2)
rate = rospy.Rate(10) # Herz
clockMessage = Clock()
while True:
clockData = "CLOCK|{0}".format(int(round(time.time())))
systemTime.publish(clockData)
clockMessage.clock = rospy.Time.now()
rosTime.publish(clockMessage)
rate.sleep()
def __init__(self):
print "Loading ROMS Data....."
start_date = rospy.get_param('start_date')
start_date = datetime.datetime.strptime(start_date, '%Y/%m/%d')
end_date = rospy.get_param('end_date')
end_date = datetime.datetime.strptime(end_date, '%Y/%m/%d')
#roms_files is an ordered list of ROMS NETCDF files containing simulated data
#The actual files containing data get located in the /data folder
# self.roms_data = {}
self.roms_dates, self.roms_files = getROMS(start_date, end_date)
self.past_roms = ROMSSnapshot(0, self.roms_dates, self.roms_files)
self.future_roms = ROMSSnapshot(1, self.roms_dates, self.roms_files)
dataset = nc.Dataset(self.roms_files[0])
self.roms_lat = dataset['lat'][:]
self.roms_lon = np.array(
[x if x < 180. else x - 360 for x in dataset['lon'][:]])
self.depth = dataset['depth'][:]
print self.depth
print len(self.roms_lat)
print len(self.roms_lon)
print len(self.depth)
roms_n_bound = np.max(self.roms_lat)
roms_s_bound = np.min(self.roms_lat)
roms_e_bound = np.max(self.roms_lon)
roms_w_bound = np.min(self.roms_lon)
self.max_depth = np.max(self.depth)
# for dt, file_path in zip(self.roms_dates, roms_files):
# self.roms_data[dt] = nc.Dataset(file_path)
print "Loading ROMS Data Complete"
print "Loading Bathymetry Data......"
bathymetry_file = getBathymetry(roms_n_bound, roms_s_bound,
roms_e_bound, roms_w_bound)
dataset = nc.Dataset(bathymetry_file)
self.bathymetry_lat = dataset['lat']
self.bathymetry_lon = dataset['lon']
self.bathymetry = dataset['Band1']
self.bathymetry_n_bound = np.max(self.bathymetry_lat)
self.bathymetry_s_bound = np.min(self.bathymetry_lat)
self.bathymetry_e_bound = np.max(self.bathymetry_lon)
self.bathymetry_w_bound = np.min(self.bathymetry_lon)
print "Loading Bathymetry Data Complete"
self.sim_factor = rospy.get_param('sim_speedup_factor')
print "Beginning Simulation with sim factor:", self.sim_factor
# print "ROMS Bounds:"
# print "\tNorth", roms_n_bound, "deg Lat"
# print "\tSouth", roms_s_bound, "deg Lat"
# print "\tEast", roms_e_bound, "deg Lon"
# print "\tWest", roms_w_bound, "deg Lon"
# print "Bathymetry Bounds:"
# print "\tNorth", bathymetry_n_bound, "deg Lat"
# print "\tSouth", bathymetry_s_bound, "deg Lat"
# print "\tEast", bathymetry_e_bound, "deg Lon"
# print "\tWest", bathymetry_w_bound, "deg Lon"
self.n_bound = min(self.bathymetry_n_bound, roms_n_bound)
self.s_bound = max(self.bathymetry_s_bound, roms_s_bound)
self.e_bound = min(self.bathymetry_e_bound, roms_e_bound)
self.w_bound = max(self.bathymetry_w_bound, roms_w_bound)
print "Simulation Bounds:"
print "\tNorth", self.n_bound, "deg Lat"
print "\tSouth", self.s_bound, "deg Lat"
print "\tEast", self.e_bound, "deg Lon"
print "\tWest", self.w_bound, "deg Lon"
rospy.set_param('sim_n_bound', float(self.n_bound))
rospy.set_param('sim_s_bound', float(self.s_bound))
rospy.set_param('sim_e_bound', float(self.e_bound))
rospy.set_param('sim_w_bound', float(self.w_bound))
print "Initializing Publishers and Services"
self.clock_pub = rospy.Publisher("/clock", Clock, queue_size=1)
rospy.Service(rospy.get_param("world_sim_topic"), SimQuery,
self.simQueryHandle)
rospy.Service('WorldSnapshot', SimSnapshot, self.simSnapshotHandle)
self.sim_time = self.roms_dates[
0] #Initialize simulated clock with beginning of ROMS Data
self.prev_time = datetime.datetime.now()
msg = Clock()
msg.clock = rospy.Time.from_sec(getTimestamp(self.sim_time))
self.clock_pub.publish(msg)
#!/usr/bin/env python
import rospy
from rosgraph_msgs.msg import Clock
from std_msgs.msg import String
rospy.init_node('sim_clock', disable_rostime=True)
pub = rospy.Publisher('/clock', Clock, queue_size=10)
r = rospy.Rate(30)
print "Simulation Clock node initialized"
while not rospy.is_shutdown():
t = rospy.Time.now()
msg = Clock()
msg.clock = t
pub.publish(msg)
r.sleep()
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)
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 default(self, ci='unused'):
msg = Clock()
msg.clock = rospy.Time.from_sec(self._sim_time)
self._sim_time += self._sim_step
self.publish(msg)
#!/usr/bin/env python
import rospy
from rosgraph_msgs.msg import Clock
from std_msgs.msg import String
rospy.init_node('cassandraBagClock', disable_rostime=True)
pub = rospy.Publisher('/clock', Clock, queue_size=10)
print "node initialized"
secs = 1593189911
usecs = 500000000
t = rospy.Time(secs, usecs)
r = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
#t = rospy.Time.now()
t = rospy.Time(secs, usecs)
msg = Clock()
msg.clock = t
print t.to_sec()
pub.publish(msg)
r.sleep()
secs = secs + 1
def default(self, ci="unused"):
msg = Clock()
msg.clock = rospy.Time.from_sec(self._sim_time)
self._sim_time += self._sim_step
self.publish(msg)
def main():
rospy.init_node('sumo_wrapper', anonymous = True)
dirname = rospkg.RosPack().get_path('mrpp_sumo')
pub = rospy.Publisher('at_node', AtNode, queue_size = 10)
pub_time = rospy.Publisher('/clock', Clock, queue_size = 10)
done = False
graph_name = rospy.get_param('/graph')
file_name = rospy.get_param('/random_string')
file_path = dirname + '/outputs/{}_command.in'.format(file_name)
file_path1 = dirname + '/outputs/{}_visits.in'.format(file_name)
g = nx.read_graphml(dirname + '/graph_ml/' + graph_name + '.graphml')
s = Sumo_Wrapper(g, file_path, file_path1)
sumo_startup = ['sumo', '-c', dirname + '/graph_sumo/{}.sumocfg'.format(graph_name), '--fcd-output', dirname + '/outputs/{}_vehicle.xml'.format(file_name)]
# sumo_startup = ['sumo-gui', '-c', dirname + '/graph_sumo/{}.sumocfg'.format(graph_name)]
traci.start(sumo_startup)
init_bots = int(rospy.get_param('/init_bots'))
while len(s.robots) < init_bots:
print ("{} robots are yet to be initialized".format(init_bots - len(s.robots)))
time.sleep(1.0)
while not done:
done = rospy.get_param('done')
s.stamp = traci.simulation.getTime()
print (s.stamp)
t = Clock()
t.clock = rospy.Time(int(s.stamp))
pub_time.publish(t)
# in_sim_robots = traci.vehicle.getIDList()
for _, w in enumerate(s.robot_rem):
if w[1] <= s.stamp and w[0] in s.robots:
s.robots.remove(w[0])
if w[0] in s.robots_stopped_already:
s.robots_stopped_already.remove(w[0])
traci.vehicle.remove(vehID = w[0])
s.routes.pop(w[0])
if len(s.robots) == 0:
print ("No robots currently in the simulator. If you wish to terminate, set rosparam done to True")
else:
stopped_bots = []
added_bots = []
for robot in s.robots:
# print (robot, s.robots_in_traci, s.routes[robot], s.stamp)
if (not robot in s.robots_in_traci) and (s.routes[robot]['d'][0] <= s.stamp):
# if not robot in in_sim_robots:
added_bots.append(robot)
# print (s.routes[robot]['r'][:1])
traci.route.add(routeID = robot + '_edges', edges = s.routes[robot]['r'][:1])
traci.vehicle.add(vehID = robot, routeID = robot + '_edges')
traci.vehicle.setStop(vehID = robot, edgeID = s.routes[robot]['r'][0], pos = traci.lane.getLength(s.routes[robot]['r'][0] + '_0'), duration = 1000.)
s.routes[robot]['r'].pop(0)
s.routes[robot]['d'].pop(0)
s.robots_in_traci.append(robot)
# in_sim_robots = traci.vehicle.getIDList()
elif traci.vehicle.isStopped(vehID = robot) and not robot in s.robots_just_stopped:
stopped_bots.append(robot)
s.robots_just_stopped.append(robot)
elif traci.vehicle.isStopped(vehID = robot) and robot in s.robots_just_stopped and not robot in s.robots_stopped_already:
# print (traci.vehicle.getStops(vehID = robot))
if len(s.routes[robot]['r']) == 0:
s.next_task_update(robot, traci.vehicle.getRoute(vehID = robot)[-1])
d = s.routes[robot]['d'].pop(0)
next_edges = [traci.vehicle.getRoute(vehID = robot)[-1], s.routes[robot]['r'].pop(0)]
traci.vehicle.setRoute(vehID = robot, edgeList = next_edges)
traci.vehicle.setStop(vehID = robot, edgeID = next_edges[0], pos = traci.lane.getLength(next_edges[0] + '_0'), duration = float(d - s.stamp))
traci.vehicle.setStop(vehID = robot, edgeID = next_edges[1], pos = traci.lane.getLength(next_edges[1] + '_0'), duration = 1000.)
s.robots_stopped_already.append(robot)
# stopped_bots.append(robot)
elif not traci.vehicle.isStopped(vehID = robot) and robot in s.robots_stopped_already:
s.robots_stopped_already.remove(robot)
s.robots_just_stopped.remove(robot)
if len(stopped_bots) + len(added_bots) > 0:
msg = AtNode()
msg.stamp = s.stamp
msg.robot_id = stopped_bots
msg.node_id = []
for w in stopped_bots:
stop_lane = traci.vehicle.getLaneID(vehID = w)
stop_edge = stop_lane[:-2]
msg.node_id.append(s.edge_nodes[stop_edge][-1])
msg.robot_id.extend(added_bots)
for w in added_bots:
stop_edge = traci.vehicle.getRoute(vehID = w)[0]
msg.node_id.append(s.edge_nodes[stop_edge][0])
pub.publish(msg)
s.output_file1.write(str(s.stamp) + '\n')
s.output_file1.write(' '.join(map(str, msg.node_id)) + '\n')
s.output_file1.write(' '.join(map(str, msg.robot_id)) + '\n')
traci.simulationStep()
s.output_file.close()
s.output_file1.close()
traci.close()
def main_function():
global msg
rospy.init_node("bagmerge_node", anonymous=True)
rospy.sleep(0.5)
for k in range(len(input_file)):
sys.stdout.write("\n%s" % input_file[k])
sys.stdout.flush()
if raw_input("\nAre you sure these are the correct files? (y/n): ") == "y":
sys.stdout.write("\n%s\n" % output_file)
sys.stdout.flush()
if os.path.exists(output_file):
if raw_input(
"Output file already exists, do you want to replace it? (y/n): "
) == "y":
os.remove(output_file)
else:
sys.exit(0)
else:
sys.exit(0)
# sys.stdout.write("\n%s" % input_file)
# sys.stdout.write("\n%s\n" % output_file)
# sys.stdout.flush()
# os.remove(output_file)
bag_in = numpy.empty(len(input_file), dtype=object)
bag_time = numpy.zeros((2, len(input_file)))
for k in range(len(input_file)):
sys.stdout.write("\nOpening bag %d... " % (k + 1))
sys.stdout.flush()
bag_in[k] = rosbag.Bag(input_file[k])
sys.stdout.write("Ok!")
sys.stdout.flush()
bag_time[1, k] = bag_in[k].get_end_time() - bag_in[k].get_start_time()
for topic, msg, t in bag_in[k].read_messages():
bag_time[0, k] = rospy.Time.to_sec(t)
break
if match_bags:
bag_start_time = min(bag_time[0])
bag_end_time = min(bag_time[0]) + max(bag_time[1])
bag_shift_time = bag_time[0] - min(bag_time[0])
else:
bag_start_time = min(bag_time[0])
bag_end_time = max(bag_time[0] + bag_time[1])
bag_shift_time = numpy.zeros(len(input_file))
sys.stdout.write(
"\n\nBag starting: %10.9f\nBag ending: %10.9f\nBag duration: %3.1fs\n"
% (bag_start_time, bag_end_time, (bag_end_time - bag_start_time)))
for k in range(len(input_file)):
sys.stdout.write("Bag %d shift: %3.1f\n" %
((k + 1), bag_shift_time[k]))
sys.stdout.flush()
sys.stdout.write("\n")
bag_out = rosbag.Bag(output_file, "w")
odom_seq = 0
k = 0
while k < len(input_file):
if "volta" in output_file:
tf_t265_init = (21.4719467163, 0.453556478024, -0.0645855739713,
-0.000818398199044, 0.00119601248298,
0.983669400215, 0.179978996515)
elif "200210_173013" in input_file[k]:
tf_t265_init = (-10.7891979218, -8.670835495, -0.227906405926,
0.00645246729255, 0.000483442592667,
0.979790627956, -0.199921101332)
else:
tf_t265_init = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
for topic, msg, t in bag_in[k].read_messages():
topic_bool = False
if rospy.is_shutdown():
k = len(input_file)
break
if match_bags and hasattr(msg, "header"):
t = rospy.Time.from_sec(
rospy.Time.to_sec(t) - bag_shift_time[k])
msg.header.stamp = copy(t)
if False:
pass
# elif topic == "/ar_pose_marker":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/ar_pose_marker_back":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/axis_back/camera_info":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/axis_back/image_raw/compressed":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/axis_front/camera_info":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/axis_front/image_raw/compressed":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
elif topic == "/cmd_vel":
topic_bool = True
bag_out.write(topic, msg, t)
#
# elif topic == "/decawave/tag_pose":
# topic_bool = True
# bag_out.write(topic, msg, t)
# uwb_msg = Odometry()
# uwb_msg.header = msg.header
# uwb_msg.pose.pose.position.x = msg.x
# uwb_msg.pose.pose.position.y = msg.y
# uwb_msg.pose.pose.position.z = msg.z
# uwb_msg.pose.pose.orientation.w = 1.0
# bag_out.write("/decawave/odom", uwb_msg, t)
#
# elif topic == "/device1/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/device2/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/device3/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/device4/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/device5/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/device6/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
elif topic == "/imu/data":
topic_bool = True
bag_out.write(topic, msg, t)
msg_wheel = copy(msg)
msg_wheel.header.frame_id = "imu_wheel_ekf"
bag_out.write("/imu_wheel_ekf/data", msg_wheel, t)
# elif topic == "/laser_odom_to_init":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# elif topic == "/integrated_to_init":
# topic_bool = True
# bag_out.write(topic, msg, t)
elif topic == "/os1_cloud_node/points":
topic_bool = True
bag_out.write(topic, msg, t)
elif topic == "/robot_odom" or topic == "/odom":
topic_bool = True
k = 0.01 + abs(msg.pose.pose.orientation.z)
msg.pose.covariance = [
k, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, k, 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, 1.0, 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,
100.0 * k
]
bag_out.write("/odom", msg, t)
elif topic == "/realsense/gyro/sample":
topic_bool = True
topic = "/t265" + topic[10:]
msg.header.frame_id = "t265" + msg.header.frame_id[9:]
ang_vel = msg.angular_velocity
elif topic == "/realsense/accel/sample":
topic_bool = True
topic = "/t265" + topic[10:]
msg.header.frame_id = "t265" + msg.header.frame_id[9:]
if "ang_vel" in locals():
imu_data = copy(msg)
imu_data.linear_acceleration.x = +copy(
msg.linear_acceleration.z)
imu_data.linear_acceleration.y = -copy(
msg.linear_acceleration.x)
imu_data.linear_acceleration.z = -copy(
msg.linear_acceleration.y)
imu_data.angular_velocity.x = +copy(ang_vel.z)
imu_data.angular_velocity.y = -copy(ang_vel.x)
imu_data.angular_velocity.z = -copy(ang_vel.y)
bag_out.write("/t265/imu/sample", imu_data, t)
elif topic == "/realsense/fisheye1/image_raw/compressed":
topic_bool = True
topic = "/left/image_raw/compressed"
msg.header.frame_id = "t265" + msg.header.frame_id[9:]
bag_out.write(topic, msg, t)
elif topic == "/realsense/fisheye1/camera_info":
topic_bool = True
topic = "/left/camera_info"
msg.header.frame_id = "t265" + msg.header.frame_id[9:]
bag_out.write(topic, msg, t)
elif topic == "/realsense/fisheye2/image_raw/compressed" or topic == "realsense/fisheye2/image_raw/compressed":
topic_bool = True
topic = "/right/image_raw/compressed"
msg.header.frame_id = "t265" + msg.header.frame_id[9:]
bag_out.write(topic, msg, t)
elif topic == "/realsense/fisheye2/camera_info" or topic == "/realsense/fisheye2/camera_info/":
topic_bool = True
topic = "/right/camera_info"
msg.header.frame_id = "t265" + msg.header.frame_id[9:]
Tx = msg.P[0] * (
2.0 *
tf_args[tf_args[:, 8] == "t265_fisheye2_frame", 1][0])
msg.P = msg.P[0:3] + (Tx, ) + msg.P[4:12]
bag_out.write(topic, msg, t)
elif topic == "/realsense/odom/sample":
topic_bool = True
topic = "/t265" + topic[10:]
msg.header.frame_id = "t265_init"
msg.child_frame_id = "t265_pose"
v_old = (msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z)
q_old = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
v_new = qv_mult(q_conjugate(tf_t265_init[3:]),
(v_old[0] - tf_t265_init[0], v_old[1] -
tf_t265_init[1], v_old[2] - tf_t265_init[2]))
q_new = qq_mult(q_conjugate(tf_t265_init[3:]), q_old)
msg.pose.pose.position.x = v_new[0]
msg.pose.pose.position.y = v_new[1]
msg.pose.pose.position.z = v_new[2]
msg.pose.pose.orientation.x = q_new[0]
msg.pose.pose.orientation.y = q_new[1]
msg.pose.pose.orientation.z = q_new[2]
msg.pose.pose.orientation.w = q_new[3]
bag_out.write(topic, msg, t)
clk_msg = Clock()
clk_msg.clock = copy(t)
bag_out.write("/clock", clk_msg, t)
elif topic == "/tf":
if msg.transforms[0].child_frame_id == "realsense_pose_frame":
topic_bool = True
msg.transforms[0].header.frame_id = "t265_init"
msg.transforms[0].child_frame_id = "t265_pose"
v_old = (msg.transforms[0].transform.translation.x,
msg.transforms[0].transform.translation.y,
msg.transforms[0].transform.translation.z)
q_old = (msg.transforms[0].transform.rotation.x,
msg.transforms[0].transform.rotation.y,
msg.transforms[0].transform.rotation.z,
msg.transforms[0].transform.rotation.w)
v_new = qv_mult(
q_conjugate(tf_t265_init[3:]),
(v_old[0] - tf_t265_init[0], v_old[1] -
tf_t265_init[1], v_old[2] - tf_t265_init[2]))
q_new = qq_mult(q_conjugate(tf_t265_init[3:]), q_old)
msg.transforms[0].transform.translation.x = v_new[0]
msg.transforms[0].transform.translation.y = v_new[1]
msg.transforms[0].transform.translation.z = v_new[2]
msg.transforms[0].transform.rotation.x = q_new[0]
msg.transforms[0].transform.rotation.y = q_new[1]
msg.transforms[0].transform.rotation.z = q_new[2]
msg.transforms[0].transform.rotation.w = q_new[3]
bag_out.write(topic, msg, t)
if "200123_163834" in input_file[k]:
odom_seq += 1
odom_msg = Odometry()
odom_msg.header = copy(msg.transforms[0].header)
odom_msg.header.seq = copy(odom_seq)
odom_msg.child_frame_id = copy(
msg.transforms[0].child_frame_id)
odom_msg.pose.pose.position = copy(
msg.transforms[0].transform.translation)
odom_msg.pose.pose.orientation = copy(
msg.transforms[0].transform.rotation)
bag_out.write("/t265/odom/sample", odom_msg, t)
# topic_bool = True
# if msg.transforms[0].child_frame_id == "estimation":
# uwb_ekf_msg = Pose()
# uwb_ekf_msg.position = copy(msg.transforms[0].transform.translation)
# uwb_ekf_msg.orientation = copy(msg.transforms[0].transform.rotation)
# bag_out.write("/espeleo/pose", uwb_ekf_msg, t)
if topic_bool:
status_time = 100.0 * (rospy.Time.to_sec(t) - bag_start_time
) / (bag_end_time - bag_start_time)
sys.stdout.write(
"\rBag %d/%d - Publishing %-28s %5.1f%%" %
((k + 1), len(input_file), topic, status_time))
sys.stdout.flush()
sys.stdout.write("\n")
sys.stdout.flush()
k += 1
sys.stdout.write("\n")
sys.stdout.flush()
for k in range(len(tf_args)):
# break
t = bag_start_time
while t < bag_end_time and not rospy.is_shutdown():
tf_msg = TransformStamped()
tf_msg.header.stamp = rospy.Time.from_sec(t)
tf_msg.header.frame_id = tf_args[k, 7]
tf_msg.child_frame_id = tf_args[k, 8]
tf_msg.transform.translation.x = tf_args[k, 0]
tf_msg.transform.translation.y = tf_args[k, 1]
tf_msg.transform.translation.z = tf_args[k, 2]
tf_msg.transform.rotation.x = tf_args[k, 3]
tf_msg.transform.rotation.y = tf_args[k, 4]
tf_msg.transform.rotation.z = tf_args[k, 5]
tf_msg.transform.rotation.w = tf_args[k, 6]
bag_out.write("/tf", TFMessage([tf_msg]), tf_msg.header.stamp)
t += 1.0 / tf_args[k, 9]
status_time = 100.0 * (t - bag_start_time) / (bag_end_time -
bag_start_time)
sys.stdout.write(
"\rTF %2d/%2d - Publishing %-28s %5.1f%%" %
((k + 1), len(tf_args), tf_args[k, 8], status_time))
sys.stdout.flush()
sys.stdout.write("\n")
sys.stdout.flush()
for k in range(len(input_file)):
bag_in[k].close()
bag_out.close()
sys.stdout.write("\nFiles Closed\n\n")
sys.stdout.flush()
def timer_cb(self, event):
self.t += self.time_factor * self.dt_ms / 1000
msg = Clock()
msg.clock = rospy.Time(self.t)
self.pub.publish(msg)
def publish_clock(pub, stamp):
clk_msg = Clock()
clk_msg.clock = stamp
pub.publish(clk_msg)
def advance(self, **inputs):
t = inputs['t']
time = Clock()
time.clock = rospy.Time.from_sec(t)
self.clockpub.publish(time)
return {}
# print 'got right camera_info'
right_camera_info_msg = msg
pass
else:
if ref_clock > t :#+ rospy.Duration.from_sec(0.5):
count_msgs = count_msgs + 1
# message = bag_messages.next()
# # publish clock only
# clock.clock = msg.header.stamp
# pub_clock.publish(clock)
else:
print t.to_sec(),count_msgs, ref_clock.to_sec(), count_frame
#publish message
# clock.clock = msg.header.stamp
# clock_msg.data = msg.header.stamp
clock.clock= ref_clock
left_image_msg.header.stamp = ref_clock
right_image_msg.header.stamp = ref_clock
left_camera_info_msg.header.stamp = ref_clock
right_camera_info_msg.header.stamp = ref_clock
pub_clock.publish(clock)
pub_scan.publish(msg)
pub_left_image.publish(left_image_msg)
pub_right_image.publish(right_image_msg)
pub_left_camera_info.publish(left_camera_info_msg)
pub_right_camera_info.publish(right_camera_info_msg)
# pub_br.sendTransform((float(words[1]) ,float(words[2]) ,float(words[3]) ), (float(words[4]) ,float(words[5]) ,float(words[6]) ,float(words[7])), rospy.Time.now(), "base_link", "odom")
print (float(words[3]) ,-float(words[1]) ,-float(words[2]) )
pub_br.sendTransform((float(words[3]) ,-float(words[1]) ,-float(words[2]) ), (float(words[6]) ,-float(words[4]) ,-float(words[5]) ,float(words[7])), rospy.Time.now(), "base_link", "odom")
#get next frame
count_frame = count_frame + 1
def publish_clock(self, t):
clock = Clock()
clock.clock = t
self.clock_pub.publish(clock)
#!/usr/bin/env python
"""
publish rospy.Time.now on /clock @ 10Hz
usage:
rosrun explore_beego fake_clock.py
"""
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('rosgraph_msgs')
import rospy
from rosgraph_msgs.msg import Clock
if __name__ == '__main__':
rospy.init_node('fake_clock')
msg = Clock()
publisher = rospy.Publisher('/clock', Clock)
while not rospy.is_shutdown():
msg.clock = rospy.Time.now()
publisher.publish(msg)
rospy.sleep(.1)
pytype = get_message_class(msg_type)
if pytype is None:
missing.append(msg_type)
continue
pubs[topic] = rospy.Publisher(topic, pytype, queue_size=1)
if missing:
missing.sort()
click.echo("Missing message types:\n" + '\n '.join(missing))
ctx.exit(2)
clock_pub = rospy.Publisher('/clock', Clock, queue_size=1)
clock_msg = Clock()
for topic, secs, nsecs, raw_msg in msc.messages:
time = genpy.Time(secs, nsecs)
clock_msg.clock = time
clock_pub.publish(clock_msg)
pub = pubs[topic]
assert pub.md5sum == raw_msg[2]
msg = pub.data_class()
msg.deserialize(raw_msg[1])
pub.publish(msg)
if rospy.is_shutdown():
click.echo('Aborting due to rospy shutdown.')
ctx.exit(108)
click.echo('Finished publishing')
rospy.signal_shutdown('Finished publishing')
rospy.spin()
def increment_clock(clock_pub, cumulative_time):
clock_signal = Clock()
clock_signal.clock = Time.from_sec(cumulative_time)
increment_clock.time = clock_signal.clock
clock_pub.publish(clock_signal)
return cumulative_time
def output(self,**inputs):
t = inputs['t']
time = Clock()
time.clock = rospy.Time.from_sec(t)
self.clockpub.publish(time)
return {}
def default(self, ci='unused'):
msg = Clock()
msg.clock = self.get_time()
self.publish(msg)
def main_function():
#global img_last, img_curr, img_depth, img_color, t_last, t_curr, t_color, t_norm, odom_msg
rospy.init_node("bagmerge_node", anonymous=True)
rospy.sleep(0.1)
for k in range(len(input_file)):
sys.stdout.write("\n%s" % input_file[k])
sys.stdout.flush()
rospy.sleep(0.01)
if raw_input("\nAre you sure these are the correct files? (y/n): ") == "y":
sys.stdout.write("\n%s\n" % output_file)
sys.stdout.flush()
if os.path.exists(output_file):
if raw_input("Output file already exists, do you want to replace it? (y/n): ") == "y":
os.remove(output_file)
else:
sys.exit(0)
else:
sys.exit(0)
bag_in = numpy.empty(len(input_file), dtype=object)
bag_time = numpy.zeros((2,len(input_file)))
for k in range(len(input_file)):
sys.stdout.write("\nOpening bag %d... " % (k+1))
sys.stdout.flush()
bag_in[k] = rosbag.Bag(input_file[k])
sys.stdout.write("Ok!")
sys.stdout.flush()
bag_time[1,k] = bag_in[k].get_end_time() - bag_in[k].get_start_time()
for topic, msg, t in bag_in[k].read_messages():
bag_time[0,k] = rospy.Time.to_sec(t)
break
if match_bags:
bag_start_time = min(bag_time[0])
bag_end_time = min(bag_time[0]) + max(bag_time[1])
bag_shift_time = bag_time[0] - min(bag_time[0])
else:
bag_start_time = min(bag_time[0])
bag_end_time = max(bag_time[0] + bag_time[1])
bag_shift_time = numpy.zeros(len(input_file))
sys.stdout.write("\n\nBag starting: %10.9f\nBag ending: %10.9f\nBag duration: %3.1fs\n" % (bag_start_time, bag_end_time, (bag_end_time-bag_start_time)))
for k in range(len(input_file)):
sys.stdout.write("Bag %d shift: %3.1f\n" % ((k+1), bag_shift_time[k]))
sys.stdout.flush()
sys.stdout.write("\n")
bag_out = rosbag.Bag(output_file, "w")
bridge = CvBridge()
img_curr = numpy.zeros((480,640),dtype="uint16")
seq_color = 0
seq_depth = 0
t_curr = 0.0
t_color = []
img_color = numpy.zeros((0,480,640,3),dtype="uint8")
pub_bool = [False, False, False, False]
k = 0
while k < len(input_file):
if "2020-02-21-17-07-46" in input_file[k]:
tf_t265_init = (0.37515220046, -0.0101201804355, 0.00968149211258, -0.0046105417423, -0.0330555289984, -0.0177801921964, 0.999284684658)
tf_wheel_init = (0.389882879089, -0.00783591647163, 0.0, 0.0, 0.0, -0.00117437191261, 0.999999310425)
else:
tf_t265_init = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
tf_wheel_init = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
for topic, msg, t in bag_in[k].read_messages():
topic_bool = False
if rospy.is_shutdown():
k = len(input_file)
break
if match_bags and hasattr(msg, "header"):
t = rospy.Time.from_sec(rospy.Time.to_sec(t) - bag_shift_time[k])
msg.header.stamp = copy(t)
# if (topic[:7]+topic[8:]) == "/device/get_joint_state":
# topic_bool = True
# bag_out.write(topic, msg, t)
#
# if topic == "/cmd_vel":
# topic_bool = True
# bag_out.write(topic, msg, t)
elif topic == "/d435i/gyro/sample":
topic_bool = True
bag_out.write(topic, msg, t)
ang_vel = copy(msg.angular_velocity)
elif topic == "/d435i/accel/sample":
topic_bool = True
bag_out.write(topic, msg, t)
lin_acc = copy(msg.linear_acceleration)
if "ang_vel" in locals():
imu_data = copy(msg)
imu_data.header.frame_id = 'rtabmap_imu'
imu_data.linear_acceleration.x = +copy(lin_acc.z)
imu_data.linear_acceleration.y = -copy(lin_acc.x)
imu_data.linear_acceleration.z = -copy(lin_acc.y)
imu_data.angular_velocity.x = +copy(ang_vel.z)
imu_data.angular_velocity.y = -copy(ang_vel.x)
imu_data.angular_velocity.z = -copy(ang_vel.y)
bag_out.write("/rtabmap/imu_raw", imu_data, t)
msg_wheel = copy(imu_data)
msg_wheel.header.frame_id = "robot_imu"
bag_out.write("/robot_imu_raw", msg_wheel, t)
elif topic == "/d435i/color/camera_info":
topic_bool = True
msg_color_info = copy(msg)
pub_bool[0] = True
elif topic == "/d435i/color/image_raw":
topic_bool = True
if "msg_color_info" in locals():
bag_out.write("/d435i/color/camera_info", msg_color_info, msg_color_info.header.stamp)
bag_out.write("/d435i/color/image_raw", msg, msg.header.stamp)
msg_color_raw = copy(msg)
pub_bool[1] = True
t_color.append(rospy.Time.to_sec(t))
img_color_raw = bridge.imgmsg_to_cv2(msg_color_raw, desired_encoding="passthrough")
img_color_raw = numpy.reshape(img_color_raw,(1,480,640,3))
img_color = numpy.concatenate((img_color, img_color_raw), axis=0)
if all(pub_bool):
seq_color += 1
msg_color_info.header.seq = copy(seq_color)
msg_color_raw.header.seq = copy(seq_color)
msg_color_info.header.stamp = copy(t)
msg_color_raw.header.stamp = copy(t)
msg_color_raw.header.frame_id = "d435i_depth_optical_frame"
msg_color_info.header.frame_id = "d435i_depth_optical_frame"
bag_out.write("/rgb/camera_info", msg_color_info, msg_color_info.header.stamp)
bag_out.write("/rgb/image_rect", msg_color_raw, msg_color_raw.header.stamp)
elif topic == "/d435i/depth/camera_info":
topic_bool = True
msg_depth_info = copy(msg)
pub_bool[2] = True
elif topic == "/d435i/depth/image_rect_raw":
topic_bool = True
if "msg_depth_info" in locals():
bag_out.write("/d435i/depth/camera_info", msg_depth_info, msg_depth_info.header.stamp)
bag_out.write("/d435i/depth/image_rect_raw", msg, msg.header.stamp)
img_last = copy(img_curr)
img_curr = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
t_last = copy(t_curr)
t_curr = rospy.Time.to_sec(t)
if all(pub_bool):
t_norm = (numpy.array(t_color)-t_last)/(t_curr-t_last)
img_depth = numpy.zeros((len(t_color),480,640),dtype="uint16")
# k = len(input_file)
# break
for n in range(len(t_color)):
img_depth[n] = (1-t_norm[n]) * img_last + t_norm[n] * img_curr
img_depth[n] = depth_gain * img_depth[n]
img_depth[n][(img_last==0) | (img_curr==0) | (img_depth[n]>10000) | (img_depth[n]<0)] = 0
# img_crop = img_depth[n,90:380,120:507]
# img_align = cv2.resize(img_crop,(640,480), interpolation = cv2.INTER_NEAREST)
img_align = img_depth[n]
seq_depth += 1
msg_align_raw = bridge.cv2_to_imgmsg(img_align, encoding="passthrough")
msg_align_raw.header.seq = copy(seq_depth)
msg_align_raw.header.stamp = rospy.Time.from_sec(t_color[n]-depth_shift_time)
msg_align_raw.header.frame_id = "d435i_depth_optical_frame"
msg_align_info = msg_depth_info
msg_align_info.header.seq = copy(seq_depth)
msg_align_info.header.stamp = rospy.Time.from_sec(t_color[n]-depth_shift_time)
msg_align_info.header.frame_id = "d435i_depth_optical_frame"
bag_out.write("/depth/camera_info", msg_align_info, msg_align_info.header.stamp)
bag_out.write("/depth/image_rect", msg_align_raw, msg_align_raw.header.stamp)
pub_bool[3] = True
t_color = []
img_color = numpy.zeros((0,480,640,3),dtype="uint8")
elif topic == "/odom":
topic_bool = True
topic = "/robot_odom"
msg.header.frame_id = "wheel_init"
msg.child_frame_id = "wheel_pose"
p_old = (-copy(msg.pose.pose.position.x), -copy(msg.pose.pose.position.y), copy(msg.pose.pose.position.z))
q_old = (copy(msg.pose.pose.orientation.x), copy(msg.pose.pose.orientation.y), copy(msg.pose.pose.orientation.z), copy(msg.pose.pose.orientation.w))
v_old = (copy(msg.twist.twist.linear.x), copy(msg.twist.twist.linear.y), copy(msg.twist.twist.linear.z))
w_old = (copy(msg.twist.twist.angular.x), copy(msg.twist.twist.angular.y), copy(msg.twist.twist.angular.z))
p_new = qv_mult(q_conjugate(tf_wheel_init[3:]), (p_old[0]-tf_wheel_init[0],p_old[1]-tf_wheel_init[1],p_old[2]-tf_wheel_init[2]))
q_new = qq_mult(q_conjugate(tf_wheel_init[3:]), copy(q_old))
v_new = qv_mult(q_conjugate(tf_wheel_init[3:]), copy(v_old))
w_new = qv_mult(q_conjugate(tf_wheel_init[3:]), copy(w_old))
msg.pose.pose.position.x = p_new[0]
msg.pose.pose.position.y = p_new[1]
msg.pose.pose.position.z = p_new[2]
msg.pose.pose.orientation.x = q_new[0]
msg.pose.pose.orientation.y = q_new[1]
msg.pose.pose.orientation.z = q_new[2]
msg.pose.pose.orientation.w = q_new[3]
msg.twist.twist.linear.x = v_new[0]
msg.twist.twist.linear.y = v_new[1]
msg.twist.twist.linear.z = v_new[2]
msg.twist.twist.angular.x = w_new[0]
msg.twist.twist.angular.y = w_new[1]
msg.twist.twist.angular.z = w_new[2]
bag_out.write(topic, msg, t)
tf_msg = TransformStamped()
tf_msg.header.stamp = copy(t)
tf_msg.header.frame_id = copy(msg.header.frame_id)
tf_msg.child_frame_id = copy(msg.child_frame_id)
tf_msg.transform.translation = copy(msg.pose.pose.position)
tf_msg.transform.rotation = copy(msg.pose.pose.orientation)
bag_out.write("/tf", TFMessage([tf_msg]), t)
# elif topic == "/os1_cloud_node/imu":
# topic_bool = True
# bag_out.write(topic, msg, t)
# elif topic == "/os1_cloud_node/points":
# topic_bool = True
# bag_out.write(topic, msg, t)
elif topic == "/t265/odom/sample":
topic_bool = True
msg.header.frame_id = "t265_init"
msg.child_frame_id = "t265_pose"
p_old = (copy(msg.pose.pose.position.x), copy(msg.pose.pose.position.y), copy(msg.pose.pose.position.z))
q_old = (copy(msg.pose.pose.orientation.x), copy(msg.pose.pose.orientation.y), copy(msg.pose.pose.orientation.z), copy(msg.pose.pose.orientation.w))
v_old = (copy(msg.twist.twist.linear.x), copy(msg.twist.twist.linear.y), copy(msg.twist.twist.linear.z))
w_old = (copy(msg.twist.twist.angular.x), copy(msg.twist.twist.angular.y), copy(msg.twist.twist.angular.z))
p_new = qv_mult(q_conjugate(tf_t265_init[3:]), (p_old[0]-tf_t265_init[0],p_old[1]-tf_t265_init[1],p_old[2]-tf_t265_init[2]))
q_new = qq_mult(q_conjugate(tf_t265_init[3:]), copy(q_old))
v_new = qv_mult(q_conjugate(tf_t265_init[3:]), copy(v_old))
w_new = qv_mult(q_conjugate(tf_t265_init[3:]), copy(w_old))
msg.pose.pose.position.x = t265_gain * p_new[0]
msg.pose.pose.position.y = t265_gain * p_new[1]
msg.pose.pose.position.z = t265_gain * p_new[2]
msg.pose.pose.orientation.x = q_new[0]
msg.pose.pose.orientation.y = q_new[1]
msg.pose.pose.orientation.z = q_new[2]
msg.pose.pose.orientation.w = q_new[3]
msg.twist.twist.linear.x = t265_gain * v_new[0]
msg.twist.twist.linear.y = t265_gain * v_new[1]
msg.twist.twist.linear.z = t265_gain * v_new[2]
msg.twist.twist.angular.x = w_new[0]
msg.twist.twist.angular.y = w_new[1]
msg.twist.twist.angular.z = w_new[2]
bag_out.write(topic, msg, t)
tf_msg = TransformStamped()
tf_msg.header.stamp = copy(t)
tf_msg.header.frame_id = copy(msg.header.frame_id)
tf_msg.child_frame_id = copy(msg.child_frame_id)
tf_msg.transform.translation = copy(msg.pose.pose.position)
tf_msg.transform.rotation = copy(msg.pose.pose.orientation)
bag_out.write("/tf", TFMessage([tf_msg]), t)
clk_msg = Clock()
clk_msg.clock = copy(t)
bag_out.write("/clock", clk_msg, t)
# pos_init = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z)
# ori_init = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
# lin_init = (msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z)
# ang_init = (msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z)
# pos_world = qv_mult(tuple(tf_t265), pos_init)
# pos_world = (pos_world[0] + tf_t265[0] + tf_chassis[0], pos_world[1] + tf_t265[1] + tf_chassis[1], pos_world[2] + tf_t265[2] + tf_chassis[2])
# ori_world = qq_mult(tuple(tf_t265), ori_init)
# lin_world = qv_mult(tuple(tf_t265), lin_init)
# ang_world = qv_mult(tuple(tf_t265), ang_init)
#
# odom_msg = copy(msg)
# odom_msg.header.frame_id = "world"
# odom_msg.pose.pose.position.x = pos_world[0]
# odom_msg.pose.pose.position.y = pos_world[1]
# odom_msg.pose.pose.position.z = pos_world[2]
# odom_msg.pose.pose.orientation.x = ori_world[0]
# odom_msg.pose.pose.orientation.y = ori_world[1]
# odom_msg.pose.pose.orientation.z = ori_world[2]
# odom_msg.pose.pose.orientation.w = ori_world[3]
# odom_msg.twist.twist.linear.x = lin_world[0]
# odom_msg.twist.twist.linear.y = lin_world[1]
# odom_msg.twist.twist.linear.z = lin_world[2]
# odom_msg.twist.twist.angular.x = ang_world[0]
# odom_msg.twist.twist.angular.y = ang_world[1]
# odom_msg.twist.twist.angular.z = ang_world[2]
# bag_out.write("/t265/odom/world", odom_msg, t)
if topic_bool:
status_time = 100.0 * (rospy.Time.to_sec(t) - bag_start_time) / (bag_end_time - bag_start_time)
sys.stdout.write("\rBag %d/%d - Publishing %-28s %5.1f%%" % ((k+1), len(input_file), topic, status_time))
sys.stdout.flush()
sys.stdout.write("\n")
sys.stdout.flush()
k += 1
sys.stdout.write("\n")
sys.stdout.flush()
for k in range(len(tf_args)):
# break
t = bag_start_time
while t < bag_end_time and not rospy.is_shutdown():
tf_msg = TransformStamped()
tf_msg.header.stamp = rospy.Time.from_sec(t)
tf_msg.header.frame_id = tf_args[k,7]
tf_msg.child_frame_id = tf_args[k,8]
tf_msg.transform.translation.x = tf_args[k,0]
tf_msg.transform.translation.y = tf_args[k,1]
tf_msg.transform.translation.z = tf_args[k,2]
tf_msg.transform.rotation.x = tf_args[k,3]
tf_msg.transform.rotation.y = tf_args[k,4]
tf_msg.transform.rotation.z = tf_args[k,5]
tf_msg.transform.rotation.w = tf_args[k,6]
bag_out.write("/tf", TFMessage([tf_msg]), tf_msg.header.stamp)
t += 1.0/tf_args[k,9]
status_time = 100.0 * (t - bag_start_time) / (bag_end_time - bag_start_time)
sys.stdout.write("\rTF %2d/%2d - Publishing %-28s %5.1f%%" % ((k+1),len(tf_args),tf_args[k,8],status_time))
sys.stdout.flush()
sys.stdout.write("\n")
sys.stdout.flush()
for k in range(len(input_file)):
bag_in[k].close()
bag_out.close()
sys.stdout.write("\nFiles Closed\n\n")
sys.stdout.flush()
