def do_convert_job2(id_robot,
id_robot_res,
id_explog, id_stream, id_episode,
filename):
rs2b_config = get_rs2b_config()
boot_config = get_boot_config()
"""
id_robot: original robot must be a ROSRobot
"""
if os.path.exists(filename):
msg = 'Output file exists: %s\n' % friendly_path(filename)
msg += 'Delete to force recreating the log.'
logger.info(msg)
return
explog = rs2b_config.explogs.instance(id_explog)
robot = boot_config.robots.instance(id_robot)
orig_robot = robot.get_inner_components()[-1]
if not isinstance(orig_robot, ROSRobot):
msg = 'Expected ROSRobot, got %s' % describe_type(robot)
raise ValueError(msg)
orig_robot.read_from_log(explog)
id_environment = explog.get_id_environment()
write_robot_observations(id_stream, filename, id_robot_res,
robot, id_episode, id_environment)
def get_observations(self, messages, queue):
"""
messages: topic -> last ROS Message
returns: an instance of RobotObservations, or None if the update is not ready.
"""
self.debug_nseen += 1
if self.use_tf:
for topic, msg, _ in queue:
if topic == '/tf':
pose = self.tfreader.update(msg)
if pose is not None:
robot_pose = pose
if self.is_ready(messages, queue):
observations = self.obs_adapter.observations_from_messages(messages)
cmds = self.cmd_adapter.commands_from_messages(messages)
if cmds is None:
# we couldn't interpret a meaningful message for use
# logger.info('Skipping because not interpretable by my cmd_adapter.')
self.debug_nskipped += 1
if self.debug_nskipped % 100 == 0:
perc = 100.0 * self.debug_nskipped / self.debug_nseen
msg = ('Skipped %6d/%6d = %.1f%% because cmd_adapter could not interpret.' %
(self.debug_nskipped, self.debug_nseen, perc))
logger.info(msg)
return None
commands_source, commands = cmds
episode_end = False
_, _, last_t = queue[-1]
timestamp = last_t.to_sec()
robot_pose = None
if self.use_odom_topic:
odometry = messages[self.odom_topic]
ros_pose = odometry.pose.pose
x = ros_pose.position.x
y = ros_pose.position.y
theta = ros_pose.orientation.z
from geometry import SE3_from_SE2, SE2_from_translation_angle
robot_pose = SE3_from_SE2(SE2_from_translation_angle([x, y], theta))
# print('odom: %s' % SE2.friendly(SE2_from_SE3(robot_pose)))
if self.prev_odom is not None:
delta = SE3.multiply(SE3.inverse(self.prev_odom), robot_pose)
# print('odom delta: %s' % SE2.friendly(SE2_from_SE3(delta)))
self.prev_odom = robot_pose
if self.use_tf:
robot_pose = self.tfreader.get_pose()
return RobotObservations(timestamp=timestamp,
observations=observations,
commands=commands, commands_source=commands_source,
episode_end=episode_end,
robot_pose=robot_pose)
else:
return None
