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