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 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 _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 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, 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 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): # 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 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 __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 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 __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 __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 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 handle_time(self, time): # time stamp at which the measurements were performed expressed in [ms] secs = time / 1000 ros_time = rospy.Time.from_seconds(secs) self.stamp = ros_time msg = Clock() msg.clock.secs = ros_time.secs msg.clock.nsecs = ros_time.nsecs self.pub_clock.publish(msg)
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 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 __init__(self, launchfile): #print(launchfile) self.last_clock_msg = Clock() self.port = "11311" # str(random_number) #os.environ["ROS_PORT_SIM"] self.port_gazebo = "11345" # str(random_number+1) #os.environ["ROS_PORT_SIM"] # self.ros_master_uri = os.environ["ROS_MASTER_URI"]; # self.port = os.environ.get("ROS_PORT_SIM", "11311") #print(f"\nROS_MASTER_URI = http://localhost:{self.port}\n") #print(f"GAZEBO_MASTER_URI = http://localhost:{self.port_gazebo}\n") 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: # TODO: Global env for 'f1'. It must be passed in constructor. fullpath = str( Path( Path(__file__).resolve().parents[1] / "CustomRobots" / "f1" / "launch" / launchfile)) print(f"-----> {fullpath}") if not os.path.exists(fullpath): raise IOError(f"File {fullpath} does not exist") # Launching Gazebo only first time tmp = os.popen("ps -Af").read() gzclient_count = tmp.count('gzclient') gzserver_count = tmp.count('gzserver') #if gzserver_count == 0: self._roslaunch = subprocess.Popen([ sys.executable, os.path.join(ros_path, b"roslaunch"), "-p", self.port, fullpath, ]) #if gzclient_count == 0: subprocess.Popen("gzclient") self.gzclient_pid = int( subprocess.check_output(["pidof", "-s", "gzclient"])) #print("Gazebo launched!") self.gzclient_pid = 0 # Launch the simulation with the given launchfile name rospy.init_node("gym", anonymous=True)
def step_physics(self, event): if self.start_time is None: self.start_time = time.time() self.clock += self.physics_dt self.clock_pub.publish(Clock(clock=rospy.Time(self.clock))) # print 'delta_t', self.clock, time.time() - self.start_time self.physics_world.step() for rendered, physical in self.entity_pairs: rendered.set_pose(physical.pose)
def _update_clock(self, carla_timestamp): """ Private Function used to perform the update of the clock :param carla_timestamp: Current Carla Timestamp :type carla_timestamp: carla.TimeStamp :return: """ self.ros_timestamp = rospy.Time.from_sec( carla_timestamp.elapsed_seconds) self.publish_ros_message('clock', Clock(self.ros_timestamp))
def clock_thread(): try: clock_pub = nh.advertise('/clock', Clock) t = genpy.Time.from_sec(12345) while True: clock_pub.publish(Clock(clock=t, )) yield util.wall_sleep(.01) t = t + genpy.Duration.from_sec(.1) except Exception: traceback.print_exc()
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 __init__(self, worldfile): self.last_clock_msg = Clock() random_number = random.randint(10000, 15000) # self.port = "11311"#str(random_number) #os.environ["ROS_PORT_SIM"] # self.port_webots = "11345"#str(random_number+1) #os.environ["ROS_PORT_SIM"] self.port = str(random_number) #os.environ["ROS_PORT_SIM"] self.port_webots = str(random_number + 1) #os.environ["ROS_PORT_SIM"] #self.port = str(11311) os.environ["ROS_MASTER_URI"] = "http://localhost:" + self.port #os.environ["Webots stream"] = "http://localhost:"+self.port_webots # # self.ros_master_uri = os.environ["ROS_MASTER_URI"]; print("ROS_MASTER_URI=http://localhost:" + self.port + "\n") print("Webots Stream=http://localhost:" + self.port_webots + "\n") # self.port = os.environ.get("ROS_PORT_SIM", "11311") #roscore_file = subprocess.check_output(["which", "roscore"]) #webots_file = (subprocess.check_output(["which", "webots"])) # 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 worldfile.startswith("/"): fullpath = worldfile else: fullpath = os.path.join(os.path.dirname(__file__), "assets", "worlds", worldfile) if not os.path.exists(fullpath): raise IOError("File " + fullpath + " does not exist") self._roscore = subprocess.Popen(["roscore", "-p", self.port]) #self._roscore = subprocess.Popen(["rosparam", "set", "/use_sim_time", "true"]) port_param = '--stream="port=' + self.port_webots + '"' stdout_param = '--stdout' self._webots = subprocess.Popen([ "webots", "--batch", "--no-sandbox", "--stderr", port_param, fullpath ]) print("Webots launched!") self.webots_pid = 0 # Launch the simulation with the given launchfile name rospy.init_node('gym', anonymous=True)
def update_clock(self, carla_timestamp): """ perform the update of the clock :param carla_timestamp: the current carla time :type carla_timestamp: carla.Timestamp :return: """ self.ros_timestamp = rospy.Time.from_sec( carla_timestamp.elapsed_seconds) self.publish_message('clock', Clock(self.ros_timestamp))
def update_clock(self, carla_timestamp): """ perform the update of the clock :param carla_timestamp: the current carla time :type carla_timestamp: carla.Timestamp :return: """ self.last_timestamp = self.ros_timestamp self.ros_timestamp = rospy.Time.from_sec(carla_timestamp) self.sim_time_pub.publish(Clock(self.ros_timestamp))
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 faketime(): rospy.set_param('/use_sim_time', True) pub = rospy.Publisher('/clock', Clock, queue_size=0) rospy.init_node('fake_time') c = Clock() while not rospy.is_shutdown(): float_secs = time.time() secs = int(float_secs) c.clock.nsecs = int((float_secs - secs) * 1000000000) c.clock.secs = secs pub.publish(c) time.sleep(0.01)