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 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 _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 _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)
class ClockReceiver:
def __init__(self, host, port=8888):
rospy.init_node('clock_receiver', anonymous=False)
rospy.on_shutdown(self.__shutdown)
if rospy.has_param("use_sim_time"):
# print "use_sim_time: {}".format(rospy.get_param("use_sim_time"))
rospy.delete_param("use_sim_time")
rospy.set_param("use_sim_time", 1)
self.time_tcpCliSock = socket(AF_INET, SOCK_STREAM)
addr = (host, port)
self.time_tcpCliSock.connect(addr)
print "time_tcp连接成功"
# clock pub
self.clock_pub = rospy.Publisher("clock", Clock, queue_size=1000)
self.clock = Clock()
self.isStop = False
def run(self):
while not self.isStop:
try:
data = self.time_tcpCliSock.recv(8)
except Exception, msg:
print msg
continue
try:
self.clock.deserialize(data)
self.clock_pub.publish(self.clock)
except genpy.DeserializationError:
print rospy.loginfo("deserialize time failed!")
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 tick_and_tock(self): self.sim.pre_update() self.sim.physics_update() self.time += self.time_step cmsg = ClockMsg() cmsg.clock = self.time self.clock_pub.publish(cmsg) self.sim.post_update()
def publish_rosbag_info(self):
rosbag_start_time = Clock()
rosbag_start_time.clock.secs = int(self.rosbag_start_time)
rosbag_start_time.clock.nsecs = int(
(self.rosbag_start_time % 1) * (10**9))
rosbag_end_time = Clock()
rosbag_end_time.clock.secs = int(self.rosbag_end_time)
rosbag_end_time.clock.nsecs = int((self.rosbag_end_time % 1) * (10**9))
self.start_time_publisher.publish(rosbag_start_time)
self.end_time_publisher.publish(rosbag_end_time)
def run(self):
rospy.loginfo('Starting with the send rate ' + str(self.clockSendRate))
# Get the current time
startWallclockTime = time.time()
currentWallclockTime = startWallclockTime
# Set the current simulation time to now
startSimulationTime = startWallclockTime
currentSimulationTime = startSimulationTime
# Sleep period - self is how long to sleep before sending out
# the next clock message
sleepWallclockPeriod = 1.0 / float(self.clockSendRate)
# Simulation time increment with each message
simulationTimeIncrement = self.timeScaleFactor * sleepWallclockPeriod
# Get the next sleep time and seed it
sleepUntilWallclockTime = startWallclockTime + sleepWallclockPeriod
# Message to send
clockMessage = Clock()
# Next debug message time to print
debugMessageWallclockTime = currentWallclockTime
while not rospy.is_shutdown():
# Publish the simulation time
clockMessage.clock = rospy.Time.from_sec(currentSimulationTime)
self.clockPublisher.publish(clockMessage)
# Print debug message if required
if (debugMessageWallclockTime <= currentWallclockTime):
rospy.loginfo('Real duration=' + str(currentWallclockTime - startSimulationTime) \
+ '; simulated duration=' + str(currentSimulationTime - startSimulationTime))
debugMessageWallclockTime += 2
# Work out how long to sleep for. If we've missed a sleep
# period, thrown a warning and skip it
currentWallclockTime = time.time()
timeToSleep = sleepUntilWallclockTime - currentWallclockTime
if (timeToSleep < 0):
rospy.logwarn('Cannot keep up; timeToSleep=' +
str(timeToSleep) + '; skipping sleep step')
sleepUntilWallclockTime = currentWallclockTime
else:
time.sleep(timeToSleep)
# Work out when the next sleep time will be
sleepUntilWallclockTime += sleepWallclockPeriod
# Advance the simulation time
currentSimulationTime += simulationTimeIncrement
def clockLoop(self):
while not rospy.is_shutdown():
enabledNow = self.enabled
if enabledNow != self.wasEnabled:
if enabledNow:
rospy.loginfo(
"Starting to publish simulated time on /clock")
if self.loop:
rospy.loginfo(
"Simulated time will be reset to start time after %.1f seconds"
% self.simulationLength)
rospy.loginfo(
"Setting ROS parameter /use_sim_time to TRUE")
self.lastWallTime = time.time()
self.currentSimulationOffset = 0.0
self.wasSimTimeEnabled = rospy.get_param(
"/use_sim_time", False)
rospy.set_param("/use_sim_time", True)
else:
rospy.loginfo(
"Stopping to publish simulated time on /clock")
rospy.set_param("/use_sim_time", self.wasSimTimeEnabled)
rospy.loginfo("Resetting ROS parameter /use_sim_time to " +
str(self.wasSimTimeEnabled).upper())
self.wasEnabled = enabledNow
if enabledNow:
currentWallTime = time.time()
wallTimePassed = currentWallTime - self.lastWallTime
if not self.paused:
self.currentSimulationOffset += wallTimePassed * self.simulationRate
if self.currentSimulationOffset > self.simulationLength:
if self.loop:
self.currentSimulationOffset %= self.simulationLength
rospy.loginfo(
"Maximum simulation length (%.1f sec) reached, restarting simulation"
% self.simulationLength)
else:
self.simulationEnded = True
clock = Clock()
clock.clock = rospy.Time(self.simulationStart +
self.currentSimulationOffset)
try:
self.clockPublisher.publish(clock)
except rospy.exceptions.ROSException as e:
pass
self.lastWallTime = currentWallTime
time.sleep(1.0 / 100)
def increment_time(self):
self.current_sim_time += self.args.dt
if self.args.no_ros:
pass
else:
cmsg = Clock()
cmsg.clock = self.get_sim_time()
self.clockpub.publish(cmsg)
# store the moment the simulation was started
if self.sim_start_walltime is None:
self.sim_start_walltime = self.get_walltime()
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 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 simtime_talker():
pub1 = rospy.Publisher('clock',Clock, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
sim_speed_multiplier = 1
sim_clock = Clock()
zero_time = rospy.get_time()
while not rospy.is_shutdown():
print('sss')
sim_clock.clock = rospy.Time.from_sec(sim_speed_multiplier*(rospy.get_time() - zero_time))
rospy.loginfo(sim_clock)
pub1.publish(sim_clock)
rate.sleep()
def stepSimulator(self):
current_time = datetime.datetime.now()
elapsed = current_time - self.prev_time
self.sim_time += elapsed * self.sim_factor
msg = Clock()
msg.clock = rospy.Time.from_sec(getTimestamp(self.sim_time))
self.clock_pub.publish(msg)
self.prev_time = current_time
if self.sim_time > self.future_roms.date:
print "ROMS Rollover"
#Increment Simulator
self.past_roms = self.future_roms
self.future_roms = ROMSSnapshot(self.past_roms.idx + 1,
self.roms_dates, self.roms_files)
def output(self, **inputs):
t = inputs['t']
time = Clock()
time.clock.secs = int(t)
time.clock.nsecs = int(float(t - int(t)) * 1000000000)
self.clockpub.publish(time)
return {}
def run(self, running):
self.init()
start = self.start_time - self.pre_roll
end = self.end_time + self.post_roll
# start value
clock_msg = Clock(clock=start)
# timing details, should be moved to constructor parameters
updates_hz = 2000.0
rate = rospy.Rate(updates_hz)
# this assumes close to real-time playback
update = rospy.Duration(1.0 / updates_hz)
# wait for the signal to start
self.event.wait()
while running.value and clock_msg.clock <= end:
# update time
clock_msg.clock += update
# publish time
self.clock_pub.publish(clock_msg)
rate.sleep()
rospy.logdebug('Playback clock finished')
running.value = False
def handle_real_time(self, time):
# real unix time stamp at which the measurements were performed in [ms]
msg = Clock()
msg.clock.secs = time // 1000
msg.clock.nsecs = (time % 1000) * 10 ** 6
# if self.base_frame == 'robot1':
self.pub_server_time_clock.publish(msg)
def __init__(self, ros_active=False, mode='normal'):
# requires WEBOTS_ROBOT_NAME to be set to "amy" or "rory"
self.ros_active = ros_active
self.time = 0
self.clock_msg = Clock()
self.supervisor = Supervisor()
self.amy_node = self.supervisor.getFromDef("amy")
self.rory_node = self.supervisor.getFromDef("rory")
self.jack_node = self.supervisor.getFromDef("jack")
self.donna_node = self.supervisor.getFromDef("donna")
self.melody_node = self.supervisor.getFromDef("melody")
if mode == 'normal':
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
elif mode == 'paused':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
elif mode == 'fast':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST)
else:
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
self.motors = []
self.sensors = []
self.timestep = int(self.supervisor.getBasicTimeStep())
# resolve the node for corresponding name
self.robot_names = ["amy", "rory", "jack", "donna", "melody"]
self.robot_nodes = {}
self.translation_fields = {}
self.rotation_fields = {}
# check if None
for name in self.robot_names:
node = self.supervisor.getFromDef(name)
if node is not None:
self.robot_nodes[name] = node
self.translation_fields[name] = node.getField("translation")
self.rotation_fields[name] = node.getField("rotation")
if self.ros_active:
rospy.init_node("webots_ros_supervisor", argv=['clock:=/clock'])
self.clock_publisher = rospy.Publisher("/clock",
Clock,
queue_size=1)
self.model_state_publisher = rospy.Publisher("/model_states",
ModelStates,
queue_size=1)
self.reset_service = rospy.Service("reset", Empty, self.reset)
self.initial_poses_service = rospy.Service("initial_pose", Empty,
self.set_initial_poses)
self.set_robot_position_service = rospy.Service(
"set_robot_position", SetRobotPose, self.robot_pose_callback)
self.reset_ball_service = rospy.Service("reset_ball", Empty,
self.reset_ball)
self.world_info = self.supervisor.getFromDef("world_info")
self.ball = self.supervisor.getFromDef("ball")
def __init__(self, launchfile):
self.last_clock_msg = Clock()
self.port = os.environ.get("ROS_PORT_SIM", "11311")
ros_path = os.path.dirname(
subprocess.check_output(["which", "roscore"]))
# NOTE: It doesn't make sense to launch a roscore because it will be done when spawing Gazebo, which also need
# to be the first node in order to initialize the clock.
# # start roscore with same python version as current script
# self._roscore = subprocess.Popen([sys.executable, os.path.join(ros_path, b"roscore"), "-p", self.port])
# time.sleep(1)
# print ("Roscore launched!")
if launchfile.startswith("/"):
fullpath = launchfile
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets",
"launch", launchfile)
if not os.path.exists(fullpath):
raise IOError("File " + fullpath + " does not exist")
self._roslaunch = subprocess.Popen([
sys.executable,
os.path.join(ros_path, b"roslaunch"), "-p", self.port, fullpath
])
print("Gazebo launched!")
time.sleep(5)
self.gzclient_pid = 0
# Launch the simulation with the given launchfile name
rospy.init_node('gym', anonymous=True)
def __init__(self):
# Launch the simulation with the given launchfile name
gazebo_env.GazeboEnv.__init__(self, "GazeboProject.launch")
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
#self.before_pose = rospy.Publisher('/gazebo/before_model_states', Pose, queue_size=5)
rospy.Subscriber('/gazebo/model_states', ModelStates, self.update_pose)
rospy.Subscriber('/clock', Clock, self.sim_time)
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation',
Empty)
self.action_space = spaces.Discrete(4) #F,L,R,B
self.reward_range = (-np.inf, np.inf)
self.sim_time = Clock()
self.action_time = 0.00000
self.pose = Pose()
self.goalpose = Pose()
self.beforepose = Pose()
self.startpose = Pose()
self._seed()
self.goal = False
self.randomstart = False
self.goalpose.x = 2.000 # map0: 1.5, map1: 1.25, map2: 2.0
self.goalpose.y = -0.250 # map0,1: 0.0, map2: -0.25
self.get_pose(self.beforepose)
def test_clock(self):
"""
Tests clock
"""
rospy.init_node('test_node', anonymous=True)
clock_msg = rospy.wait_for_message("/clock", Clock, timeout=TIMEOUT)
self.assertNotEqual(Clock(), clock_msg)
def step(self, ms):
if self.get_parameter('use_joint_state_publisher').value:
self.jointStatePublisher.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.clockPublisher.publish(msg)
def __init__(self, launchfile):
self.last_clock_msg = Clock()
random_number = random.randint(10000, 15000)
self.port = str(random_number) #os.environ["ROS_PORT_SIM"]
self.port_gazebo = str(random_number + 1) #os.environ["ROS_PORT_SIM"]
os.environ["ROS_MASTER_URI"] = "http://localhost:" + self.port
os.environ[
"GAZEBO_MASTER_URI"] = "http://localhost:" + self.port_gazebo
# self.ros_master_uri = os.environ["ROS_MASTER_URI"];
print("ROS_MASTER_URI=http://localhost:" + self.port + "\n")
print("GAZEBO_MASTER_URI=http://localhost:" + self.port_gazebo + "\n")
# self.port = os.environ.get("ROS_PORT_SIM", "11311")
ros_path = os.path.dirname(
subprocess.check_output(["which", "roscore"]))
if launchfile.startswith("/"):
fullpath = launchfile
else:
fullpath = os.path.join(os.path.dirname(__file__), "assets",
"launch", launchfile)
if not os.path.exists(fullpath):
raise IOError("File " + fullpath + " does not exist")
self._roslaunch = subprocess.Popen([
sys.executable,
os.path.join(ros_path, b"roslaunch"), "-p", self.port, fullpath
])
print("Gazebo launched!")
self.gzclient_pid = 0
def tick(self, timer_event):
self.time += self.time_step
cmsg = ClockMsg()
cmsg.clock = self.time
self.clock_pub.publish(cmsg)
super(AFAPSimulator, self).tick(None)
now = time()
if self.__last_tick != None:
deltaT = now - self.__last_tick
ratio = self.time_step.to_sec() / deltaT
sys.stdout.write(self.terminal.move(self.terminal.height, 0))
msg = u'Simulation Factor: {}'.format(ratio)
self.__last_msg_len = len(msg)
sys.stdout.write(msg)
sys.stdout.flush()
self.__last_tick = now
def offline(args):
from rosgraph_msgs.msg import Clock
from bruce_slam.utils import io
io.offline = True
cimgs = []
logwarn("Press s to save image")
clock_pub = rospy.Publisher("/clock", Clock, queue_size=100)
for topic, msg in read_bag(args.file, args.start, args.duration):
while not rospy.is_shutdown():
if callback_lock_event.wait(1.0):
break
if rospy.is_shutdown():
break
if topic == SONAR_TOPIC:
node.sonar_sub.callback(msg)
if node.feature_img is not None:
cv2.imshow("feature image", node.feature_img)
key = cv2.waitKey()
if key == 27:
break
elif key == ord("s"):
loginfo("Save current frame")
cimgs.append(node.cimg)
else:
continue
clock_pub.publish(Clock(msg.header.stamp))
if cimgs:
np.savez("cimgs.npz", cimgs=cimgs)
def __init__(self):
# Launch the simulation with the given launchfile name
gazebo_env.GazeboEnv.__init__(self, "GazeboProject.launch")
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
rospy.Subscriber('/gazebo/model_states', ModelStates, self.update_pose)
rospy.Subscriber('/clock', Clock, self.sim_time)
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation',
Empty)
self.reward_range = (-np.inf, np.inf)
self.sim_time = Clock()
self.action_time = 0.00000
self.pose = Pose()
self.goalpose = Pose()
self.beforepose = Pose()
self.startpose = Pose()
self.before_avg_data = 0
self._seed()
self.goal = False
self.goalpose.x = 15.000 # map0: 1.5, map1: 1.25, map2: 2.0
self.goalpose.y = 0.000 # map0,1: 0.0, map2: -0.25
self.get_pose(self.beforepose)
self.subgoal_as_dist_to_goal = 1000 # max. lidar's value
def offline(args):
from localization_node import LocalizationNode
from rosgraph_msgs.msg import Clock
from bruce_slam.utils import io
io.offline = True
loc_node = LocalizationNode()
loc_node.init_node("/bruce/localization/")
clock_pub = rospy.Publisher("/clock", Clock, queue_size=100)
for topic, msg in read_bag(args.file,
args.start,
args.duration,
progress=True):
while not rospy.is_shutdown():
if callback_lock_event.wait(1.0):
break
if rospy.is_shutdown():
break
if topic == IMU_TOPIC:
loc_node.imu_sub.callback(msg)
elif topic == DVL_TOPIC:
loc_node.dvl_sub.callback(msg)
elif topic == DEPTH_TOPIC:
loc_node.depth_sub.callback(msg)
elif topic == SONAR_TOPIC:
node.sonar_sub.callback(msg)
clock_pub.publish(Clock(msg.header.stamp))
def __init__(self):
'''
Initialization of the class.
'''
self.start_flag = False # flag indicates if the first measurement is received
self.config_start = False # flag indicates if the config callback is called for the first time
self.euler_mv = Vector3() # measured euler angles
self.euler_sp = Vector3(0, 0, 0) # euler angles referent values
self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller
self.euler_rate_mv = Vector3() # measured angular velocities
self.clock = Clock()
self.pid_roll = PID() # roll controller
self.pid_pitch = PID() # pitch controller
##################################################################
##################################################################
# Add your PID params here
self.pid_roll.set_kp(5.0)
self.pid_roll.set_ki(6.0)
self.pid_roll.set_kd(0.0)
self.pid_pitch.set_kp(5.0)
self.pid_pitch.set_ki(6.0)
self.pid_pitch.set_kd(0.0)
self.joint0 = [0, -45, -45, 0, -45, -45, 0, -45, -45, 0, -45, -45]
self.joint_ref = copy.deepcopy(self.joint0)
self.joint_msg = JointState()
##################################################################
##################################################################
self.rate = 20.0
self.ros_rate = rospy.Rate(self.rate) # attitude control at 20 Hz
self.t_old = 0
rospy.Subscriber('imu', Imu, self.ahrs_cb)
rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb)
rospy.Subscriber('/clock', Clock, self.clock_cb)
self.pub_joint_references = rospy.Publisher('aquashoko_chatter',
JointState,
queue_size=1)
self.pub_pid_roll = rospy.Publisher('pid_roll',
PIDController,
queue_size=1)
self.pub_pid_pitch = rospy.Publisher('pid_pitch',
PIDController,
queue_size=1)
self.cfg_server = Server(AttitudeControlParamsConfig,
self.cfg_callback)
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 _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 offline(args):
from rosgraph_msgs.msg import Clock
from localization_node import LocalizationNode
from feature_extraction_node import FeatureExtractionNode
from mapping_node import MappingNode
from bruce_slam.utils import io
io.offline = True
node.save_fig = False
node.save_data = False
loc_node = LocalizationNode()
loc_node.init_node(SLAM_NS + "localization/")
fe_node = FeatureExtractionNode()
fe_node.init_node(SLAM_NS + "feature_extraction/")
mp_node = MappingNode()
mp_node.init_node(SLAM_NS + "mapping/")
clock_pub = rospy.Publisher("/clock", Clock, queue_size=100)
for topic, msg in read_bag(args.file,
args.start,
args.duration,
progress=True):
while not rospy.is_shutdown():
if callback_lock_event.wait(1.0):
break
if rospy.is_shutdown():
break
if topic == IMU_TOPIC:
loc_node.imu_sub.callback(msg)
elif topic == DVL_TOPIC:
loc_node.dvl_sub.callback(msg)
elif topic == DEPTH_TOPIC:
loc_node.depth_sub.callback(msg)
elif topic == SONAR_TOPIC:
fe_node.sonar_sub.callback(msg)
if node.sonar_sub.callback:
node.sonar_sub.callback(msg)
mp_node.sonar_sub.callback(msg)
clock_pub.publish(Clock(msg.header.stamp))
# Publish map to world so we can visualize all in a z-down frame in rviz.
node.tf.sendTransform((0, 0, 0), [1, 0, 0, 0], msg.header.stamp, "map",
"world")
# Save trajectory and point cloud
import os
import datetime
stamp = datetime.datetime.now()
name = os.path.basename(args.file).split(".")[0]
np.savez(
"run-{}@{}.npz".format(name, stamp),
poses=node.get_states(),
points=np.c_[node.get_points(return_keys=True)],
)
def update_clock(self):
self.sim_time += rospy.Duration.from_sec(self.wallclock_time_step *
self.time_factor)
c = Clock(self.sim_time)
self.clock_publisher.publish(c)
if not rospy.is_shutdown():
clock_timer = threading.Timer(self.wallclock_time_step,
self.update_clock)
clock_timer.start()
def TimeStampPublisher(self, ):
t = rospy.Time.from_sec(self.timestamps[self.timestamp_idx][1])
time_msg = Clock()
time_msg.clock = t
info_str = "publishing index %d" % self.timestamp_idx
rospy.loginfo(info_str + " stamp= " +
str(self.timestamps[self.timestamp_idx][1]))
label_file = self.timestamps[self.timestamp_idx][0].split('/')[-1]
label_file = label_file.split('.')[0]
label_file_path = self.labels_base_dir + "/" + self.sequence + "/" + label_file + ".txt"
bboxes_msg = self.GetBBoxesMsg(label_file_path)
bboxes_msg.header.stamp = t
#print(t)
# also publish bounding boxes
self.timestamp_pub.publish(time_msg)
self.gt_boundingbox_pub.publish(bboxes_msg)
def step_physics(self, event):
self.clock += self.physics_dt
self.clock_pub.publish(Clock(clock=rospy.Time(self.clock)))
self.clock += self.physics_dt
self.physics_world.step()
if self.draw:
for rendered, physical in self.entity_pairs:
rendered.set_pose(physical.pose)
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 default(self, ci='unused'):
msg = Clock()
msg.clock = self.get_time()
self.publish(msg)
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 = rospy.Time.from_sec(self._sim_time)
self._sim_time += self._sim_step
self.publish(msg)
pubs = {}
missing = []
for topic, msg_type in topics.items():
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)
from sensor_msgs.msg import Image, CompressedImage
from sensor_msgs.msg import CameraInfo
print sys.argv[1]
print sys.argv[2]
bag = rosbag.Bag(sys.argv[1])
f = open(sys.argv[2], "r")
lines = f.readlines()
print bag
left_image_topic = '/camera/left/image_rect_color/compressed'
right_image_topic = '/camera/right/image_rect_color/compressed'
left_camera_info_topic = '/camera/left/camera_info'
right_camera_info_topic = '/camera/right/camera_info'
# bag_messages = bag.read_messages(topics=['/scan', left_image_topic , right_image_topic, left_image_topic, left_camera_info_topic])
# print lines
print 'processing trajectory...'
clock = Clock()
clock_msg = Time()
rospy.init_node('recordedFrameSyncPublisher', anonymous=False)
pub_clock = rospy.Publisher('/clock', Clock)
pub_scan = rospy.Publisher('/scan', LaserScan)
pub_right_image = rospy.Publisher(right_image_topic, CompressedImage)
pub_left_image = rospy.Publisher(left_image_topic, CompressedImage)
pub_right_camera_info = rospy.Publisher(right_camera_info_topic, CameraInfo)
pub_left_camera_info = rospy.Publisher(left_camera_info_topic, CameraInfo)
pub_br = tf.TransformBroadcaster()
# for topic, msg, t in bag.read_messages(topics=['/scan', '/odom_pose_do_not_subscribe', '/tf']):
#first message
# (topic,msg,t) = bag_messages.next()
# clock.clock = msg.header.stamp
# clock_msg.data = msg.header.stamp
count_msgs = 0
#!/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)
#!/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()
