def ros_timestamp(sec=0, nsec=0, from_sec=False):
time = Time()
if from_sec:
time.sec = int(sec)
time.nanosec = int((sec - int(sec)) * 1000000000)
else:
time.sec = int(sec)
time.nanosec = int(nsec)
return time
def get_time_msg(self):
time_msg = Time()
msg_time = self.get_clock().now().seconds_nanoseconds()
time_msg.sec = int(msg_time[0])
time_msg.nanosec = int(msg_time[1])
return time_msg
def timeStamp():
current_time = time.time()
t = Time()
t.sec = int(current_time)
t.nanosec = int(current_time % 1 * 1E9)
return t
def step_callback(self):
self.robot.step(self.timestep.value)
epoch = time.time()
stamp = Time()
stamp.sec = int(epoch)
stamp.nanosec = int((epoch - int(epoch)) * 1E9)
self.odometry_callback(stamp)
self.distance_callback(stamp)
self.publish_static_transforms(stamp)
def get_time_msg(self, image_path):
time_msg = Time()
msg_time = self.get_clock().now().seconds_nanoseconds()
time_msg.sec = int(msg_time[0])
time_msg.nanosec = int(msg_time[1])
# use timestamp from image title
# path = os.path.normpath(image_path)
# folders = path.split(os.sep)
# title = folders[-1]
# title_stamp = title.split('-')[0].split('.')
# time_msg.sec = int(title_stamp[0])
# time_msg.nanosec = int(title_stamp[1])
return time_msg
def cbWheelsCmd(self, msg):
if self.estop:
self.driver.setWheelsSpeed(left=0.0, right=0.0)
return
self.driver.setWheelsSpeed(left=msg.vel_left, right=msg.vel_right)
# Put the wheel commands in a message and publish
self.msg_wheels_cmd.header = msg.header
# Record the time the command was given to the wheels_driver
current_time = time.time()
timestamp = Time()
timestamp.sec = int(current_time)
timestamp.nanosec = int(current_time % 1 * 1E9)
self.msg_wheels_cmd.header.stamp = timestamp
self.msg_wheels_cmd.vel_left = msg.vel_left
self.msg_wheels_cmd.vel_right = msg.vel_right
self.pub_wheels_cmd.publish(self.msg_wheels_cmd)
def get_current_time_msg():
current_time = time.time()
time_msg = Time()
time_msg.sec = int(current_time)
time_msg.nanosec = int(current_time % 1 * 1E9)
return time_msg
def _get_stamp(self, time_float):
time_frac_int = modf(time_float)
stamp = Time()
stamp.sec = int(time_frac_int[1])
stamp.nanosec = int(time_frac_int[0] * 1000000000) & 0xffffffff
return stamp
def get_current_time():
current_time = Time()
current_time.sec = int(time.time())
current_time.nanosec = int((time.time() - current_time.sec) * 1000000000)
return current_time
def getTimestamp(self, kuka_timestamp):
timestamp = Time()
timestamp.sec = math.floor(kuka_timestamp)
timestamp.nanosec = int((kuka_timestamp - timestamp.sec) * 10**9)
return timestamp
