def _get_earlist_index(self, index_list):
is_last = False
earlist_index = -1
index = 0
queue_list = []
for queue in self.fusion_operator.queues_for_input_ports.values():
queue_list.append(queue)
index = index + 1
index = 0
for queue in queue_list:
if index_list[index] != None:
if earlist_index < 0:
earlist_index = index
elif len(queue) > 0:
new = queue[index_list[index]]
new_stamp = new.header.stamp
new_stamp = Time.from_msg(new_stamp)
cur_queue = queue_list[earlist_index]
cur = cur_queue[index_list[earlist_index]]
cur_stamp = cur.header.stamp
cur_stamp = Time.from_msg(cur_stamp)
if (new_stamp < cur_stamp):
earlist_index = index
index = index + 1
if len(queue_list[earlist_index]) == index_list[earlist_index] + 1:
is_last = True
return earlist_index, is_last
def fusion_callback(self, channel, msg):
try:
start = time.time()
self.fusion_operator.queues_for_input_ports[channel].append(msg)
valid_input_data = self._find_valid_input_data()
oldest_birthmark = None
if valid_input_data:
for c, data in valid_input_data.items():
if data == None:
continue
if oldest_birthmark == None or Time.from_msg(
data.header.stamp) < Time.from_msg(
oldest_birthmark):
oldest_birthmark = data.header.stamp
freshness_constraint = data.freshness_constraint
valid_input_data[c] = pickle.loads(data.body)
splash_message = SplashMessage()
header = Header()
header.stamp = oldest_birthmark
splash_message.header = header
splash_message.body = array('B',
pickle.dumps(valid_input_data))
splash_message.freshness_constraint = freshness_constraint
self.fusion_operator.stream_output_ports[
self.fusion_operator.output_channel].write(splash_message)
else:
pass
print(f"time: {(time.time() - start) * 1000:.2f}ms")
except Exception as e:
print(traceback.format_exc())
print(e)
def _heartbeat_callback(self, msg):
self.get_logger().debug('Heartbeat received {}'.format(msg.stamp))
current = Time.from_msg(self._clock.now().to_msg())
if (current - Time.from_msg(msg.stamp)) > self._timeout_duration:
return
self._set_connected(True)
self._timer.reset()
def test_time_message_conversions(self):
time1 = Time(nanoseconds=1, clock_type=ClockType.ROS_TIME)
builtins_msg = Builtins()
builtins_msg.time_value = time1.to_msg()
# Default clock type resulting from from_msg will be ROS time
time2 = Time.from_msg(builtins_msg.time_value)
assert isinstance(time2, Time)
assert time1 == time2
# Clock type can be specified if appropriate
time3 = Time.from_msg(builtins_msg.time_value, clock_type=ClockType.SYSTEM_TIME)
assert time3.clock_type == ClockType.SYSTEM_TIME
def write_estimated_pose_timer_callback(self):
try:
transform_msg = self.tf_buffer.lookup_transform(
'map', 'base_link', Time())
orientation = transform_msg.transform.rotation
theta, _, _ = pyquaternion.Quaternion(
x=orientation.x,
y=orientation.y,
z=orientation.z,
w=orientation.w).yaw_pitch_roll
self.estimated_poses_df = self.estimated_poses_df.append(
{
't':
nanoseconds_to_seconds(
Time.from_msg(transform_msg.header.stamp).nanoseconds),
'x':
transform_msg.transform.translation.x,
'y':
transform_msg.transform.translation.y,
'theta':
theta
},
ignore_index=True)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
pass
def ground_truth_pose_callback(self, odometry_msg: nav_msgs.msg.Odometry):
self.latest_ground_truth_pose_msg = odometry_msg
if not self.run_started:
return
orientation = odometry_msg.pose.pose.orientation
theta, _, _ = pyquaternion.Quaternion(x=orientation.x,
y=orientation.y,
z=orientation.z,
w=orientation.w).yaw_pitch_roll
self.ground_truth_poses_df = self.ground_truth_poses_df.append(
{
't':
nanoseconds_to_seconds(
Time.from_msg(odometry_msg.header.stamp).nanoseconds),
'x':
odometry_msg.pose.pose.position.x,
'y':
odometry_msg.pose.pose.position.y,
'theta':
theta,
'v_x':
odometry_msg.twist.twist.linear.x,
'v_y':
odometry_msg.twist.twist.linear.y,
'v_theta':
odometry_msg.twist.twist.angular.z,
},
ignore_index=True)
def read_data_frames(bag_view: BagView,
field_dict: Dict[str, List[str]],
auto_stamp=True):
data = {}
field_types = {}
topics = dict(bag_view.topics())
for (topic, fields) in field_dict.items():
data[topic] = {}
field_types[topic] = {}
msg_type = topics[topic]
fields_and_types = msg_type.get_fields_and_field_types()
if ('header.stamp' not in fields and auto_stamp
and 'header' in fields_and_types
and fields_and_types['header'] == 'std_msgs/Header'):
fields = fields.copy() # ensure not to mutate the input
fields.append('header.stamp')
for field in fields:
field_types[topic][field] = _field_type(msg_type, field)
data[topic][field] = []
for (topic, msg, _) in bag_view:
for field, field_type in field_types[topic].items():
value = _rgetattr(msg, field)
if field_type == 'builtin_interfaces/Time':
ns = Time.from_msg(value).nanoseconds
value = pd.to_datetime(ns, origin='unix', unit='ns')
data[topic][field].append(value)
return {
topic: pd.DataFrame(data_dict)
for topic, data_dict in data.items()
}
def add(self, msg):
if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
if not self.allow_headerless:
msg_filters_logger = rclpy.logging.get_logger(
'message_filters_cache')
msg_filters_logger.set_level(LoggingSeverity.INFO)
msg_filters_logger.warn("can not use message filters messages "
"without timestamp infomation when "
"'allow_headerless' is disabled. "
"auto assign ROSTIME to headerless "
"messages once enabling constructor "
"option of 'allow_headerless'.")
return
stamp = ROSClock().now()
else:
stamp = msg.header.stamp
if not hasattr(stamp, 'nanoseconds'):
stamp = Time.from_msg(stamp)
# Insert sorted
self.cache_times.append(stamp)
self.cache_msgs.append(msg)
# Implement a ring buffer, discard older if oversized
if (len(self.cache_msgs) > self.cache_size):
del self.cache_msgs[0]
del self.cache_times[0]
# Signal new input
self.signalMessage(msg)
def cb(self, msg: JointState):
"""Save the latest published movement values with corresponding timestamp."""
self._timestamp = Time.from_msg(msg.header.stamp)
self._joint_names = msg.name
self._position = msg.position
self._velocity = msg.velocity
self._effort = msg.effort
def _message_callback(self, message):
"""Log when a message is received."""
now = self.get_clock().now()
diff = now - Time.from_msg(message.header.stamp)
self.get_logger().info(
f'I heard an Image. Message single trip latency: [{diff.nanoseconds}]\n---'
)
def add(self, msg, my_queue, my_queue_index=None):
if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
if not self.allow_headerless:
msg_filters_logger = rclpy.logging.get_logger(
'message_filters_approx')
msg_filters_logger.set_level(LoggingSeverity.INFO)
msg_filters_logger.warn(
"can not use message filters for "
"messages without timestamp infomation when "
"'allow_headerless' is disabled. auto assign "
"ROSTIME to headerless messages once enabling "
"constructor option of 'allow_headerless'.")
return
stamp = ROSClock().now()
else:
stamp = msg.header.stamp
if not hasattr(stamp, 'nanoseconds'):
stamp = Time.from_msg(stamp)
# print(stamp)
self.lock.acquire()
my_queue[stamp.nanoseconds] = msg
while len(my_queue) > self.queue_size:
del my_queue[min(my_queue)]
# self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...]
if my_queue_index is None:
search_queues = self.queues
else:
search_queues = self.queues[:my_queue_index] + \
self.queues[my_queue_index+1:]
# sort and leave only reasonable stamps for synchronization
stamps = []
for queue in search_queues:
topic_stamps = []
for s in queue:
stamp_delta = Duration(nanoseconds=abs(s - stamp.nanoseconds))
if stamp_delta > self.slop:
continue # far over the slop
topic_stamps.append(
((Time(nanoseconds=s,
clock_type=stamp.clock_type)), stamp_delta))
if not topic_stamps:
self.lock.release()
return
topic_stamps = sorted(topic_stamps, key=lambda x: x[1])
stamps.append(topic_stamps)
for vv in itertools.product(*[list(zip(*s))[0] for s in stamps]):
vv = list(vv)
# insert the new message
if my_queue_index is not None:
vv.insert(my_queue_index, stamp)
qt = list(zip(self.queues, vv))
if (((max(vv) - min(vv)) < self.slop) and
(len([1 for q, t in qt if t.nanoseconds not in q]) == 0)):
msgs = [q[t.nanoseconds] for q, t in qt]
self.signalMessage(*msgs)
for q, t in qt:
del q[t.nanoseconds]
break # fast finish after the synchronization
self.lock.release()
def _task(self):
if len(self._queue) > 0:
# print("*"*5, len(self._queue))
msg = self._queue.pop(0)
# self._count_queue.append(msg)
self.count += 1
self.count_rate += 1
time_diff_ms = (
self._stream_output_port.parent.get_clock().now().nanoseconds -
Time.from_msg(msg.header.stamp).nanoseconds) / 1000000
if msg.freshness > 0 and msg.freshness < time_diff_ms:
self.count_fresh += 1
# raise self.exception('{}ms exceeded(constraint: {}ms, cur: {}ms'.format(time_diff_ms - msg.freshness, msg.freshness, time_diff_ms))
self._task()
return
tlqkf1 = time.time()
self.count_publish += 1
# print("rate publish: ", tlqkf1 - self.tlqkf1_prev)
self._publisher.publish(msg)
self.tlqkf1_prev = tlqkf1
elif self._last_sent_item:
self.count += 1
msg = self._last_sent_item
else:
return
self._last_sent_item = msg
def callback_delay(self, msg):
"""
Calculate delay time.
:param msg: Message instance
"""
if not hasattr(msg, 'header'):
raise RuntimeError('msg does not have header')
with self.lock:
curr_rostime = self._clock.now()
# time reset
if curr_rostime.nanoseconds == 0:
if len(self.delays) > 0:
print('time has reset, resetting counters')
self.delays = []
return
curr = curr_rostime.nanoseconds
if self.msg_t0 < 0 or self.msg_t0 > curr:
self.msg_t0 = curr
self.msg_tn = curr
self.delays = []
else:
# store the duration nanoseconds in self.delays
duration = (curr_rostime - Time.from_msg(msg.header.stamp))
self.delays.append(duration.nanoseconds)
self.msg_tn = curr
if len(self.delays) > self.window_size:
self.delays.pop(0)
def _check_mode_and_execute_callback(self, msg):
if self.parent.mode == self.parent.get_current_mode():
# self.parent.get_logger().info(msg.body)
# self.parent.get_logger().info(msg.header.frame_id)
if msg.freshness:
# self.parent.get_logger().info("freshness: {}".format(msg.freshness))
time_exec_ms = (
self.parent.get_clock().now().nanoseconds -
Time.from_msg(msg.header.stamp).nanoseconds) / 1000000
# self.parent.get_logger().info("time_exec: {}".format(time_exec_ms))
if time_exec_ms > msg.freshness:
self.get_logger.warn(
'{}ms exceeded(constraint: {}ms, cur: {}ms'.format(
time_exec_ms - msg.freshness, msg.freshness,
time_exec_ms))
self.msg_list.append(msg)
msg_decoded = json.loads(msg.body)
msg_converted = convert_dictionary_to_ros_message(
self._msg_type, msg_decoded)
if self._args:
self._callback(msg, self._args[0])
else:
if self.from_fusion:
# self.parent.get_logger().info("{}".format(msg_converted))
msg_obj = self.FusionedObj(json.loads(msg_converted.data))
self._callback(msg_obj)
else:
self._callback(msg_converted)
self.msg_list.pop(0)
else:
pass
def update_state(self, msg: AsebaEvent) -> None:
if self.state == CalibrationState.waiting_button:
return
if self.state == CalibrationState.waiting_ground:
self.ths = [d - self.gap for d in msg.data]
self.state = CalibrationState.running
return
if self.motor_speed == 0:
return
line = self.line
if self.motor == 'right':
v = msg.data[0]
th = self.ths[0]
else:
v = msg.data[1]
th = self.ths[1]
self.get_logger().info(f'update_state {line.state} {v} {th} -> ?')
stamp = Time.from_msg(msg.stamp).nanoseconds * 1e-9
if v > th and line.state == LineState.UNKNOWN:
line.state = LineState.OFF
if v < th and line.state == LineState.OFF:
line.state = LineState.IN
line.in_at_time = stamp
if v > th and line.state == LineState.IN:
line.state = LineState.OFF
pick = (stamp + line.in_at_time) / 2
self.get_logger().info(f"Line after {pick} s")
self.add_pick(pick)
def lookup_transform(
self,
target_frame: str,
source_frame: str,
time: Time,
timeout: Duration = Duration()
) -> TransformStamped:
"""
Get the transform from the source frame to the target frame.
:param target_frame: Name of the frame to transform into.
:param source_frame: Name of the input frame.
:param time: The time at which to get the transform (0 will get the latest).
:param timeout: Time to wait for the target frame to become available.
:return: The transform between the frames.
"""
if isinstance(time, builtin_interfaces.msg.Time):
source_time = Time.from_msg(time)
warnings.warn(
'Passing a builtin_interfaces.msg.Time argument is deprecated, and will be removed in the near future. '
'Use rclpy.time.Time instead.')
elif isinstance(time, Time):
source_time = time
else:
raise TypeError('Must pass a rclpy.time.Time object.')
goal = LookupTransform.Goal()
goal.target_frame = target_frame
goal.source_frame = source_frame
goal.source_time = source_time.to_msg()
goal.timeout = timeout.to_msg()
goal.advanced = False
return self.__process_goal(goal)
def process(self, args, msgs):
idx = 0
with open(args.out, 'w') as f:
for _, img_msg, stamp in msgs:
if args.header:
stamp = Time.from_msg(img_msg.header.stamp).nanoseconds
f.write(f'{str(idx).zfill(8)},{stamp}\n')
idx += 1
def test_time_message_conversions_big_nanoseconds(self):
time1 = Time(nanoseconds=1553575413247045598, clock_type=ClockType.ROS_TIME)
builtins_msg = Builtins()
builtins_msg.time_value = time1.to_msg()
# Default clock type resulting from from_msg will be ROS time
time2 = Time.from_msg(builtins_msg.time_value)
assert isinstance(time2, Time)
assert time1 == time2
def _is_valid_data(self, index_list):
i = 0
cur_data_list = []
for queue in self.fusion_operator.queues_for_input_ports.values():
if index_list[i] != None:
cur_data_list.append(queue[index_list[i]])
i = i + 1
if len(cur_data_list) < 2:
return False
for data in cur_data_list:
for data2 in cur_data_list:
if data == data2: continue
time_diff_ms = abs(
Time.from_msg(data.header.stamp).nanoseconds -
Time.from_msg(data2.header.stamp).nanoseconds) / 1000000
if time_diff_ms > self.fusion_operator.fusion_rule.correlation_constraint:
return False
return True
def _check_and_fusion(self, msg, channel):
# print("=====================================")
# print("check_and_fusion: ", channel)
try:
msg_decoded = json.loads(msg.body)
msg_converted = convert_dictionary_to_ros_message(
self._stream_input_ports[channel][0].get_msg_type(),
msg_decoded)
for c, queue in self._queues_for_each_input_port.items():
index = 0
new_queue = []
for item in queue:
time_exec_ms = (self.get_clock().now().nanoseconds -
item["time"]) / 1000000
if item["freshness"] == None or item[
"freshness"] == 0 or item[
"freshness"] > time_exec_ms:
new_queue.append(item)
else:
self.get_logger.warn(
'{}ms exceeded(constraint: {}ms, cur: {}ms'.format(
time_exec_ms - item["freshness"],
item["freshness"], time_exec_ms))
index = index + 1
self._queues_for_each_input_port[c] = new_queue
self._queues_for_each_input_port[channel].append({
"message":
msg_converted,
"time":
Time.from_msg(msg.header.stamp).nanoseconds,
"freshness":
msg.freshness
})
valid_input_data = self._find_valid_input_data(
self._fusion_rule, self._queues_for_each_input_port)
if valid_input_data:
# print(valid_input_data)
data_encoded = json.dumps(valid_input_data)
else:
# print("==============empty data===============")
return
# empty_input_data = {}
# for c in self._queues_for_each_input_port.keys():
# empty_input_data[c] = None
# data_encoded = json.dumps(empty_input_data)
new_msg = String()
new_msg.data = data_encoded
for port in self._stream_output_ports:
port.write(new_msg)
# print("================{}================".format(valid_input_data))
except Exception as e:
pass
def _velocity(self, prev_pose, current_pose, prev_stamp, current_stamp):
# 姿勢の差分から速度を求める
velocity = Pose2D()
diff_time_stamp = Time.from_msg(current_stamp) - Time.from_msg(
prev_stamp)
diff_time_secs = diff_time_stamp.nanoseconds / 1000000000
if diff_time_secs > 0:
diff_pose = Pose2D()
diff_pose.x = current_pose.x - prev_pose.x
diff_pose.y = current_pose.y - prev_pose.y
diff_pose.theta = self._angle_normalize(current_pose.theta -
prev_pose.theta)
velocity.x = diff_pose.x / diff_time_secs
velocity.y = diff_pose.y / diff_time_secs
velocity.theta = diff_pose.theta / diff_time_secs
return velocity
def process(self, args, odometry_iter):
with open(args.out, 'w') as f:
fmt = '{0:.' + str(args.precision) + 'f}'
# if the input is a geo pose it is UTM-projected, and we need to detect
# (zone, band) changes from message to message
previous_utm_zone_band = None
for topic_name, msg, _ in odometry_iter:
if isinstance(msg, Odometry):
pos = msg.pose.pose.position
ori = msg.pose.pose.orientation
elif isinstance(msg, NavSatFix):
utm_point = fromLatLong(msg.latitude, msg.longitude,
msg.altitude)
pos = Vector3()
pos.x = utm_point.easting
pos.y = utm_point.northing
pos.z = utm_point.altitude
utm_lattice_coordinate = (utm_point.zone, utm_point.band)
if previous_utm_zone_band and previous_utm_zone_band != utm_lattice_coordinate:
msg = f'UTM (zone, band) changes between messages in topic {topic_name}'
raise ExporterError(msg)
previous_utm_zone_band = utm_lattice_coordinate
# orientation is unit for NavSatFix
ori = Quaternion()
else:
exp_clz = self.__class__.__name__
msg_clz = msg.__class__.__name__
raise TypeError(
f'{exp_clz} can not export messages of type {msg_clz}')
t_ros = Time.from_msg(msg.header.stamp)
t_sec = t_ros.nanoseconds / 1e9
f.write(str(t_sec))
f.write(' ')
f.write(fmt.format(pos.x))
f.write(' ')
f.write(fmt.format(pos.y))
f.write(' ')
f.write(fmt.format(pos.z))
f.write(' ')
f.write(fmt.format(ori.x))
f.write(' ')
f.write(fmt.format(ori.y))
f.write(' ')
f.write(fmt.format(ori.z))
f.write(' ')
f.write(fmt.format(ori.w))
f.write('\n')
def execute_callback(self, goal_handle):
response = LookupTransform.Result()
response.transform = TransformStamped()
response.error = TF2Error()
try:
if not goal_handle.request.advanced:
transform = self.buffer_core.lookup_transform_core(
target_frame=goal_handle.request.target_frame,
source_frame=goal_handle.request.source_frame,
time=Time.from_msg(goal_handle.request.source_time))
else:
transform = self.buffer_core.lookup_transform_full_core(
target_frame=goal_handle.request.target_frame,
target_time=Time.from_msg(goal_handle.request.target_time),
source_frame=goal_handle.request.source_frame,
source_time=Time.from_msg(goal_handle.request.source_time),
fixed_frame=goal_handle.request.fixed_frame)
response.transform = transform
except LookupException as e:
response.error.error = TF2Error.LOOKUP_ERROR
return response
def _extract_robot_pose(self, color, detection_robots, time_stamp):
# ロボット姿勢を抽出する
# ロボット姿勢が複数ある場合は、平均値を姿勢とする
detections = [[] for i in range(len(self._robot_info[color]))]
# ID毎のリストに分ける
for robot in detection_robots:
robot_id = robot.robot_id
detections[robot_id].append(robot)
for robot_id, robots in enumerate(detections):
if robots:
average_pose = self._average_pose(robots)
velocity = self._velocity(
self._robot_info[color][robot_id].pose, average_pose,
self._robot_info[color][robot_id].detection_stamp,
time_stamp)
self._robot_info[color][robot_id].robot_id = robot_id
self._robot_info[color][robot_id].pose = average_pose
self._robot_info[color][
robot_id].last_detection_pose = average_pose
self._robot_info[color][robot_id].velocity = velocity
self._robot_info[color][robot_id].detected = True
self._robot_info[color][robot_id].detection_stamp = time_stamp
self._robot_info[color][robot_id].disappeared = False
else:
self._robot_info[color][robot_id].detected = False
self._robot_info[color][robot_id].robot_id = robot_id
# 座標を受け取らなかった場合は、速度を用いて線形予測する
if self._robot_info[color][robot_id].disappeared is False:
duration = Clock(clock_type=ClockType.ROS_TIME).now() - \
Time.from_msg(self._robot_info[color][robot_id].detection_stamp)
diff_time_secs = duration.nanoseconds / 1000000000
self._robot_info[color][robot_id].pose = self._estimate(
self._robot_info[color][robot_id].last_detection_pose,
self._robot_info[color][robot_id].velocity,
diff_time_secs)
# 一定時間、座標を受け取らなかったら消滅判定にする
if diff_time_secs > self._DISAPPERED_TIME_THRESH:
self._robot_info[color][robot_id].disappeared = True
if self._PUBLISH_ROBOT[color]:
for robot_id in range(len(self._pubs_robot_info[color])):
self._pubs_robot_info[color][robot_id].publish(
self._robot_info[color][robot_id])
def add(self, msg, my_queue, my_queue_index=None):
self.lock.acquire()
stamp = Time.from_msg(msg.header.stamp)
my_queue[stamp.nanoseconds] = msg
while len(my_queue) > self.queue_size:
del my_queue[min(my_queue)]
# common is the set of timestamps that occur in all queues
common = reduce(set.intersection, [set(q) for q in self.queues])
for t in sorted(common):
# msgs is list of msgs (one from each queue) with stamp t
msgs = [q[t] for q in self.queues]
self.signalMessage(*msgs)
for q in self.queues:
del q[t]
self.lock.release()
def _task(self):
if len(self._queue) > 0:
msg = self._queue.pop(0)
time_diff_ms = (self._stream_output_port.component.get_clock().now(
).nanoseconds - Time.from_msg(
msg.header.stamp).nanoseconds) / 1000000
if msg.freshness > 0 and msg.freshness < time_diff_ms:
raise self.exception(
'{}ms exceeded(constraint: {}ms, cur: {}ms'.format(
time_diff_ms - msg.freshness, msg.freshness,
time_diff_ms))
self._task()
return
elif self._last_sent_item:
msg = self._last_sent_item
else:
return
self._last_sent_item = msg
self._publisher.publish(msg)
def estimated_pose_correction_callback(
self, pose_with_covariance_msg: geometry_msgs.msg.
PoseWithCovarianceStamped):
if not self.run_started:
return
orientation = pose_with_covariance_msg.pose.pose.orientation
theta, _, _ = pyquaternion.Quaternion(x=orientation.x,
y=orientation.y,
z=orientation.z,
w=orientation.w).yaw_pitch_roll
covariance_mat = np.array(
pose_with_covariance_msg.pose.covariance).reshape(6, 6)
self.estimated_correction_poses_df = self.estimated_correction_poses_df.append(
{
't':
nanoseconds_to_seconds(
Time.from_msg(
pose_with_covariance_msg.header.stamp).nanoseconds),
'x':
pose_with_covariance_msg.pose.pose.position.x,
'y':
pose_with_covariance_msg.pose.pose.position.y,
'theta':
theta,
'cov_x_x':
covariance_mat[0, 0],
'cov_x_y':
covariance_mat[0, 1],
'cov_y_y':
covariance_mat[1, 1],
'cov_theta_theta':
covariance_mat[5, 5]
},
ignore_index=True)
def _extract_ball_pose(self, detection_balls, time_stamp):
# ボール座標を抽出する
# ボール座標が複数ある場合は、平均値を座標とする
if detection_balls:
average_pose = self._average_pose(detection_balls)
velocity = self._velocity(self._ball_info.pose, average_pose,
self._ball_info.detection_stamp,
time_stamp)
self._ball_info.pose = average_pose
self._ball_info.last_detection_pose = average_pose
self._ball_info.velocity = velocity
self._ball_info.detected = True
self._ball_info.detection_stamp = time_stamp
self._ball_info.disappeared = False
else:
self._ball_info.detected = False
if self._ball_info.disappeared is False:
# 座標を受け取らなかった場合は、速度を用いて線形予測する
diff_time_stamp = Clock(
clock_type=ClockType.ROS_TIME).now() - Time.from_msg(
self._ball_info.detection_stamp)
diff_time_secs = diff_time_stamp.nanoseconds / 1000000000
self._ball_info.pose = self._estimate(
self._ball_info.last_detection_pose,
self._ball_info.velocity, diff_time_secs)
# 一定時間、座標を受け取らなかったら消滅判定にする
if diff_time_secs > self._DISAPPERED_TIME_THRESH:
self._ball_info.disappeared = True
if self._PUBLISH_BALL:
self._pub_ball_info.publish(self._ball_info)
def clock_callback(self, msg):
# Cache the last message in case a new clock is attached.
time_from_msg = Time.from_msg(msg.clock)
self._last_time_set = time_from_msg
for clock in self._associated_clocks:
clock.set_ros_time_override(time_from_msg)
def main(self, *, args): # noqa: D102
if not os.path.exists(args.bag_file):
return print_error("bag file '{}' does not exist!".format(
args.bag_file))
if not args.topic:
args.topic = []
reader = SequentialReader()
in_storage_options = StorageOptions(uri=args.bag_file,
storage_id=args.storage)
in_converter_options = ConverterOptions(
input_serialization_format=args.serialization_format,
output_serialization_format=args.serialization_format)
reader.open(in_storage_options, in_converter_options)
info = Info()
metadata = info.read_metadata(args.bag_file, args.storage)
message_counts = {}
for entry in metadata.topics_with_message_count:
message_counts[entry.topic_metadata.name] = entry.message_count
bag_duration_s = metadata.duration.total_seconds()
type_name_to_type_map = {}
topic_to_type_map = {}
summaries = {}
for topic_metadata in reader.get_all_topics_and_types():
if args.topic and topic_metadata.name not in args.topic:
continue
if topic_metadata.type not in type_name_to_type_map:
try:
type_name_to_type_map[topic_metadata.type] = get_message(
topic_metadata.type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError(
f"Cannot load message type '{topic_metadata.type}'")
topic_to_type_map[topic_metadata.name] = type_name_to_type_map[
topic_metadata.type]
summaries[topic_metadata.name] = {
'frame_ids': set(),
'write_delays_ns': [],
'custom': default_summary_output(topic_metadata.type)
}
reader.set_filter(StorageFilter(topics=args.topic))
progress = ProgressTracker()
if args.progress:
progress.add_estimated_work(metadata, 1.0)
while reader.has_next():
(topic, data, t) = reader.read_next()
msg_type = topic_to_type_map[topic]
msg = deserialize_message(data, msg_type)
for custom in summaries[topic]['custom']:
custom.update(msg)
if hasattr(msg, 'header'):
summaries[topic]['frame_ids'].add(msg.header.frame_id)
delay = t - Time.from_msg(msg.header.stamp).nanoseconds
summaries[topic]['write_delays_ns'].append(delay)
if args.progress:
progress.print_update(progress.update(topic), every=100)
if args.progress:
progress.print_finish()
for topic, summary in summaries.items():
print(topic)
if not message_counts[topic]:
print('\tNo messages')
continue
frame_id_str = ', '.join(summary['frame_ids'])
print(f'\tframe_id: {frame_id_str}')
freq = message_counts[topic] / bag_duration_s
print(f'\tfrequency: {freq:.2f} hz')
if summary['write_delays_ns']:
# only messages with header.stamp have delays
write_delays = np.array(summary['write_delays_ns'])
delay_ms_mean = np.mean(write_delays) / 1000 / 1000
delay_ms_stddev = np.std(write_delays) / 1000 / 1000
print(
f'\twrite delay: {delay_ms_mean:.2f}ms (stddev {delay_ms_stddev:.2f})'
)
for custom in summaries[topic]['custom']:
custom.write()
