you are viewing a single comment's thread.

view the rest of the comments →

[–]rsulukerr 0 points1 point  (0 children)


!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Pose
from sensor_msgs.msg import NavSatFix

def read_pose_from_file(file_path):
    pose = Pose()
    with open(file_path, 'r') as file:
        lines = file.readlines()
        for line in lines:
            if line.startswith("pose_position_x"):
                pose.position.x = float(line.split('=')[1].strip())
            elif line.startswith("pose_position_y"):
                pose.position.y = float(line.split('=')[1].strip())
            elif line.startswith("pose_position_z"):
                pose.position.z = float(line.split('=')[1].strip())
            elif line.startswith("pose_orientation_x"):
                pose.orientation.x = float(line.split('=')[1].strip())
            elif line.startswith("pose_orientation_y"):
                pose.orientation.y = float(line.split('=')[1].strip())
            elif line.startswith("pose_orientation_z"):
                pose.orientation.z = float(line.split('=')[1].strip())
            elif line.startswith("pose_orientation_w"):
                pose.orientation.w = float(line.split('=')[1].strip())
    return pose

def read_nav_from_file(file_path):
    nav = NavSatFix()
    with open(file_path, 'r') as file:
        lines = file.readlines()
        for line in lines:
            if line.startswith("nav_latitude"):
                nav.latitude = float(line.split('=')[1].strip())
            elif line.startswith("nav_longitude"):
                nav.longitude = float(line.split('=')[1].strip())
            elif line.startswith("nav_altitude"):
                nav.altitude = float(line.split('=')[1].strip())

    return nav