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 init_points(self): if self._tf_buffer.can_transform('odom', 'front_point', Time()): transform = self._tf_buffer.lookup_transform( 'odom', 'front_point', Time()) self.front_point.position.x = transform.transform.translation.x self.front_point.position.y = transform.transform.translation.y self.front_point.position.z = transform.transform.translation.z else: self.publish_transform( 'odom', 'front_point', tf.translation_matrix((0, 0, 0))) if self._tf_buffer.can_transform('odom', 'rear_point', Time()): transform = self._tf_buffer.lookup_transform( 'odom', 'rear_point', Time()) self.rear_point.position.x = transform.transform.translation.x self.rear_point.position.y = transform.transform.translation.y self.rear_point.position.z = transform.transform.translation.z else: self.publish_transform( 'odom', 'rear_point', tf.translation_matrix((0, 0, 0))) while self.pub.get_subscription_count() < 1: pass sleep(2) self.publish_transform('odom', 'base_link', self.base_link_start_height)
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 __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) self.measurement_source_name = kwargs.get('measurement_source_name', str()) self.metrics_source = kwargs.get('metrics_source', str()) self.unit = kwargs.get('unit', str()) from builtin_interfaces.msg import Time self.window_start = kwargs.get('window_start', Time()) from builtin_interfaces.msg import Time self.window_stop = kwargs.get('window_stop', Time()) self.statistics = kwargs.get('statistics', [])
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 tf_publisher_callback(self): # Publish TF for the next step # we use one step in advance to make sure no sensor data are published before tFMessage = TFMessage() nextTime = self.robot.getTime() + 0.001 * self.timestep nextSec = int(nextTime) # rounding prevents precision issues that can cause problems with ROS timers nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6) for name in self.nodes: position = self.nodes[name].getPosition() orientation = self.nodes[name].getOrientation() transformStamped = TransformStamped() transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec) transformStamped.header.frame_id = 'map' transformStamped.child_frame_id = name transformStamped.transform.translation.x = position[0] transformStamped.transform.translation.y = position[1] transformStamped.transform.translation.z = position[2] qw = math.sqrt(1.0 + orientation[0] + orientation[4] + orientation[8]) / 2.0 qx = (orientation[7] - orientation[5]) / (4.0 * qw) qy = (orientation[2] - orientation[6]) / (4.0 * qw) qz = (orientation[3] - orientation[1]) / (4.0 * qw) transformStamped.transform.rotation.x = qx transformStamped.transform.rotation.y = qy transformStamped.transform.rotation.z = qz transformStamped.transform.rotation.w = qw tFMessage.transforms.append(transformStamped) self.tfPublisher.publish(tFMessage)
def camera_marker(self): marker = Marker() marker.header.frame_id = "world" marker.header.stamp = Time() marker.ns = "" marker.id = 0 marker.type = 10 marker.action = 0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.65 quat = euler_to_quaternion(0.0, 0, 3.9) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.scale.x = 0.00025 marker.scale.y = 0.00075 marker.scale.z = 0.00075 c = ColorRGBA() c.a = 1.0 c.r = 0.5 c.g = 0.5 c.b = 0.5 marker.color = c marker.mesh_resource = "file://" + self.photoneo_mesh return marker
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) self.accepted = kwargs.get('accepted', bool()) from builtin_interfaces.msg import Time self.stamp = kwargs.get('stamp', Time())
def publish_tf(self): # modded from https://github.com/cyberbotics/webots_ros2/blob/master/webots_ros2_core/webots_ros2_core/tf_publisher.py # Publish TF for the next step # we use one step in advance to make sure no sensor data are published before nextTime = self.robot.getTime() + 0.001 * self.timestep nextSec = int(nextTime) # rounding prevents precision issues that can cause problems with ROS timers nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6) node = self.robot.getSelf() if node is None: print("No self node") return position = node.getPosition() orientation = node.getOrientation() transformStamped = TransformStamped() transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec) transformStamped.header.frame_id = 'map' transformStamped.child_frame_id = self.robot.getName() transformStamped.transform.translation.x = position[0] transformStamped.transform.translation.y = position[2] # ROS is Z-up transformStamped.transform.translation.z = position[1] qw = math.sqrt(1.0 + orientation[0] + orientation[4] + orientation[8]) / 2.0 qx = (orientation[7] - orientation[5]) / (4.0 * qw) qy = (orientation[2] - orientation[6]) / (4.0 * qw) qz = (orientation[3] - orientation[1]) / (4.0 * qw) transformStamped.transform.rotation.x = qx transformStamped.transform.rotation.y = qy transformStamped.transform.rotation.z = qz transformStamped.transform.rotation.w = qw self.pub.publish(TFMessage(transforms=[transformStamped]))
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 publish(self, lidar, transforms): """Publish the laser scan topics with up to date value.""" nextTime = self.robot.getTime() + 0.001 * self.timestep nextSec = int(nextTime) # rounding prevents precision issues that can cause problems with ROS timers nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6) for i in range(lidar.getNumberOfLayers()): name = self.prefix + lidar.getName() + '_scan' if lidar.getNumberOfLayers() > 1: name += '_' + str(i) # publish the lidar to scan transform transformStamped = TransformStamped() transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec) transformStamped.header.frame_id = self.prefix + lidar.getName() transformStamped.child_frame_id = name q1 = transforms3d.quaternions.axangle2quat([0, 1, 0], -1.5708) q2 = transforms3d.quaternions.axangle2quat([1, 0, 0], 1.5708) result = transforms3d.quaternions.qmult(q1, q2) if lidar.getNumberOfLayers() > 1: angleStep = lidar.getVerticalFov() / (lidar.getNumberOfLayers() - 1) angle = -0.5 * lidar.getVerticalFov() + i * angleStep q3 = transforms3d.quaternions.axangle2quat([0, 0, 1], angle) result = transforms3d.quaternions.qmult(result, q3) transformStamped.transform.rotation.x = result[0] transformStamped.transform.rotation.y = result[1] transformStamped.transform.rotation.z = result[2] transformStamped.transform.rotation.w = result[3] transforms.append(transformStamped) # publish the actual laser scan msg = LaserScan() msg.header.stamp = Time(sec=self.node.sec, nanosec=self.node.nanosec) msg.header.frame_id = name msg.angle_min = -0.5 * lidar.getFov() msg.angle_max = 0.5 * lidar.getFov() msg.angle_increment = lidar.getFov() / (lidar.getHorizontalResolution() - 1) msg.scan_time = lidar.getSamplingPeriod() / 1000.0 msg.range_min = lidar.getMinRange() msg.range_max = lidar.getMaxRange() lidarValues = lidar.getLayerRangeImage(i) for j in range(lidar.getHorizontalResolution()): msg.ranges.append(lidarValues[j]) if lidar.getNumberOfLayers() > 1: key = self.prefix + lidar.getName() + '_' + str(i) self.publishers[lidar][key].publish(msg) else: self.publishers[lidar].publish(msg)
def make_ingestor_state(guid: str = "test_ingestor") -> IngestorState: return IngestorState( guid=guid, time=Time(sec=0, nanosec=0), mode=IngestorState.IDLE, request_guid_queue=[], seconds_remaining=0.0, )
def make_dispenser_state(guid: str = "test_dispenser") -> DispenserState: return DispenserState( guid=guid, time=Time(sec=0, nanosec=0), mode=DispenserState.IDLE, request_guid_queue=[], seconds_remaining=0.0, )
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from builtin_interfaces.msg import Duration self.duration_value = kwargs.get('duration_value', Duration()) from builtin_interfaces.msg import Time self.time_value = kwargs.get('time_value', Time())
def __init__(self, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from builtin_interfaces.msg import Time self.time_ref = kwargs.get('time_ref', Time()) self.source = kwargs.get('source', str())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from unique_identifier_msgs.msg import UUID self.goal_id = kwargs.get('goal_id', UUID()) from builtin_interfaces.msg import Time self.stamp = kwargs.get('stamp', Time())
def test_create_from_log_msg_with_stamp(self): seconds_since_epoch = 100 msg = Log() msg.stamp = Time(sec=seconds_since_epoch, nanosec=0) msg.level = int.from_bytes(Log.DEBUG, sys.byteorder) entry = Entry.from_ros_msg(msg, use_current_time=False) self.assertEqual(entry.date_time.toSecsSinceEpoch(), seconds_since_epoch)
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 __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from builtin_interfaces.msg import Time self.time_ref = kwargs.get('time_ref', Time()) self.source = kwargs.get('source', str())
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 _init_empty(self, kwargs): """ Set up an empty map using keyword arguments. """ self.frame_id = "map" self.stamp = Time() # There is no great way to get the actual time. self.origin_x = kwargs.get('origin_x', -2.5) self.origin_y = kwargs.get('origin_y', -2.5) self.width = kwargs.get('width', 50) self.height = kwargs.get('height', 50) self.resolution = kwargs.get('resolution', .1) self.grid = np.zeros((self.height, self.width))
def __init__(self, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from builtin_interfaces.msg import Time self.map_load_time = kwargs.get('map_load_time', Time()) self.resolution = kwargs.get('resolution', float()) self.width = kwargs.get('width', int()) self.height = kwargs.get('height', int()) from geometry_msgs.msg import Pose self.origin = kwargs.get('origin', Pose())
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from builtin_interfaces.msg import Time self.stamp = kwargs.get('stamp', Time()) self.node = kwargs.get('node', str()) self.new_parameters = kwargs.get('new_parameters', []) self.changed_parameters = kwargs.get('changed_parameters', []) self.deleted_parameters = kwargs.get('deleted_parameters', [])
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from builtin_interfaces.msg import Time self.stamp = kwargs.get('stamp', Time()) self.level = kwargs.get('level', int()) self.name = kwargs.get('name', str()) self.msg = kwargs.get('msg', str()) self.file = kwargs.get('file', str()) self.function = kwargs.get('function', str()) self.line = kwargs.get('line', int())
def make_lift_state(name: str = "test_lift") -> LiftState: return LiftState( lift_name=name or "test_lift", lift_time=Time(sec=0, nanosec=0), available_floors=["L1", "L2"], current_floor="L1", destination_floor="L1", door_state=LiftState.DOOR_CLOSED, motion_state=LiftState.MOTION_STOPPED, available_modes=[LiftState.MODE_AGV], current_mode=LiftState.MODE_AGV, session_id="test_session", )
def test_approx(self): m0 = MockFilter() m1 = MockFilter() ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.1) ts.registerCallback(self.cb_collector_2msg) if 0: # Simple case, pairs of messages, make sure that they get combined for t in range(10): self.collector = [] msg0 = MockMessage(t, 33) msg1 = MockMessage(t, 34) m0.signalMessage(msg0) self.assertEqual(self.collector, []) m1.signalMessage(msg1) self.assertEqual(self.collector, [(msg0, msg1)]) # Scramble sequences of length N. Make sure that TimeSequencer recombines them. random.seed(0) for N in range(1, 10): m0 = MockFilter() m1 = MockFilter() seq0 = [ MockMessage(Time(sec=t), random.random()) for t in range(N) ] seq1 = [ MockMessage(Time(sec=t), random.random()) for t in range(N) ] # random.shuffle(seq0) ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1) ts.registerCallback(self.cb_collector_2msg) self.collector = [] for msg in random.sample(seq0, N): m0.signalMessage(msg) self.assertEqual(self.collector, []) for msg in random.sample(seq1, N): m1.signalMessage(msg) self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
def test_all_funcs(self): sub = Subscriber(self.node, String, "/empty") cache = Cache(sub, 5) msg = AnonymMsg() msg.header.stamp = Time(sec=0) cache.add(msg) msg = AnonymMsg() msg.header.stamp = Time(sec=1) cache.add(msg) msg = AnonymMsg() msg.header.stamp = Time(sec=2) cache.add(msg) msg = AnonymMsg() msg.header.stamp = Time(sec=3) cache.add(msg) msg = AnonymMsg() msg.header.stamp = Time(sec=4) cache.add(msg) l = len(cache.getInterval(2.5, 3.5)) self.assertEqual( l, 1, "invalid number of messages" + " returned in getInterval 1") l = len(cache.getInterval(2, 3)) self.assertEqual( l, 2, "invalid number of messages" + " returned in getInterval 2") l = len(cache.getInterval(0, 4)) self.assertEqual( l, 5, "invalid number of messages" + " returned in getInterval 3") s = cache.getElemAfterTime(0).header.stamp self.assertEqual(stamp_time(s), 0, "invalid msg return by getElemAfterTime") s = cache.getElemBeforeTime(3.5).header.stamp self.assertEqual(stamp_time(s), 3, "invalid msg return by getElemBeforeTime") s = cache.getLastestTime() self.assertEqual(s, 4, "invalid stamp return by getLastestTime") s = cache.getOldestTime() self.assertEqual(s, 0, "invalid stamp return by getOldestTime") # Add another msg to fill the buffer msg = AnonymMsg() msg.header.stamp = Time(sec=5) cache.add(msg) # Test that it discarded the right one s = cache.getOldestTime() self.assertEqual(s, 1, "wrong message discarded")
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 __init__(self, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from builtin_interfaces.msg import Time self.stamp = kwargs.get('stamp', Time()) from pendulum_msgs.msg import JointCommand self.command = kwargs.get('command', JointCommand()) from pendulum_msgs.msg import JointState self.state = kwargs.get('state', JointState()) self.cur_latency = kwargs.get('cur_latency', int()) self.mean_latency = kwargs.get('mean_latency', float()) self.min_latency = kwargs.get('min_latency', int()) self.max_latency = kwargs.get('max_latency', int()) self.minor_pagefaults = kwargs.get('minor_pagefaults', int()) self.major_pagefaults = kwargs.get('major_pagefaults', int())