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)