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())
