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 update_state_from_gps_message(self, gps_json: dict):
self.latest_gps_json = gps_json
svy21_xy = self._svy_transformer.transform(gps_json["lat"],
gps_json["lon"])
self.current_loc.x = svy21_xy[1]
self.current_loc.y = svy21_xy[0]
self.current_loc.yaw = float(gps_json["heading"])
t = Time()
t.sec = gps_json["timestamp"]
self.current_loc.t = t
self.offset_x = svy21_xy[1] - gps_json["x"]
self.offset_y = svy21_xy[0] - gps_json["y"]
self.battery_percentage = gps_json["battery"]
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 convert_task_request(task_request: mdl.SubmitTask):
"""
:param (obj) task_json:
:return req_msgs, error_msg
This is to convert a json task req format to a rmf_task_msgs
task_profile format. add this accordingly when a new msg field
is introduced.
The 'start time' here is refered to the "Duration" from now.
"""
# NOTE: task request should already be validated by pydantic
req_msg = SubmitTask.Request()
if task_request.priority is not None:
req_msg.description.priority.value = task_request.priority
if task_request.task_type == mdl.TaskTypeEnum.CLEAN:
desc = cast(mdl.CleanTaskDescription, task_request.description)
req_msg.description.task_type.type = TaskType.TYPE_CLEAN
req_msg.description.clean.start_waypoint = desc.cleaning_zone
elif task_request.task_type == mdl.TaskTypeEnum.LOOP:
desc = cast(mdl.LoopTaskDescription, task_request.description)
req_msg.description.task_type.type = TaskType.TYPE_LOOP
loop = Loop()
loop.num_loops = desc.num_loops
loop.start_name = desc.start_name
loop.finish_name = desc.finish_name
req_msg.description.loop = loop
elif task_request.task_type == mdl.TaskTypeEnum.DELIVERY:
desc = cast(mdl.DeliveryTaskDescription, task_request.description)
req_msg.description.task_type.type = TaskType.TYPE_DELIVERY
delivery = Delivery()
delivery.pickup_place_name = desc.pickup_place_name
delivery.pickup_dispenser = desc.pickup_dispenser
delivery.dropoff_ingestor = desc.dropoff_ingestor
delivery.dropoff_place_name = desc.dropoff_place_name
req_msg.description.delivery = delivery
else:
return None, "Invalid TaskType"
# Calc start time, convert min to sec: TODO better representation
ros_start_time = RosTime()
ros_start_time.sec = task_request.start_time
req_msg.description.start_time = ros_start_time
return req_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
def string_callback(self, publisher, values):
msg = String()
ros_timestamp = Time()
ros_timestamp.sec = time.time()
msg.data = str(values[0]) + " %s" % time.time()
publisher.publish(msg)